From 96f5c366ba02d5a62fd57c1f3fc2ddd6f2f9eb28 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Fri, 31 Aug 2018 15:51:02 +0200
Subject: [PATCH 001/194] Add changelog for v0.12 release

---
 doc/releases.dox      |  7 ++++---
 doc/releases/v0_11.md |  2 +-
 doc/releases/v0_12.md | 14 ++++++++++++++
 3 files changed, 19 insertions(+), 4 deletions(-)
 create mode 100644 doc/releases/v0_12.md

diff --git a/doc/releases.dox b/doc/releases.dox
index 3a1243dca51..acf743b1cd0 100644
--- a/doc/releases.dox
+++ b/doc/releases.dox
@@ -10,8 +10,9 @@
 
 This page lists the main changes introduced in iDynTree at each release.
 
-
-<h1>iDynTree 0.8 Series</h1>
+\li \subpage v0_12
+\li \subpage v0_11
+\li \subpage v0_10
 \li \subpage v0_8
 
-*/
\ No newline at end of file
+*/
diff --git a/doc/releases/v0_11.md b/doc/releases/v0_11.md
index a067e4e682a..9fadfbbd232 100644
--- a/doc/releases/v0_11.md
+++ b/doc/releases/v0_11.md
@@ -1,4 +1,4 @@
-iDynTree 0.11 (UNRELEASED)                                              {#v0_11}
+iDynTree 0.11 (2018-08-31)                                              {#v0_11}
 ========================
 
 [TOC]
diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
new file mode 100644
index 00000000000..fd9513aab72
--- /dev/null
+++ b/doc/releases/v0_12.md
@@ -0,0 +1,14 @@
+iDynTree 0.12 (UNRELEASED)                                              {#v0_12}
+========================
+
+[TOC]
+
+iDynTree 0.12 Release Notes
+=========================
+
+Summary
+-------
+* 
+
+Important Changes
+-----------------

From 99567f10c7383d5e3e112f3d11e8a6f08c96484f Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Fri, 7 Sep 2018 12:40:36 +0200
Subject: [PATCH 002/194] [inverse-kinematics] Added method to return the
 convex hull of the constraint on the projection of the center of mass

---
 doc/releases/v0_12.md                            |  4 ++++
 .../include/iDynTree/InverseKinematics.h         | 16 ++++++++++++++++
 src/inverse-kinematics/src/ConvexHullHelpers.cpp |  6 ++++++
 src/inverse-kinematics/src/InverseKinematics.cpp | 16 ++++++++++++++++
 4 files changed, 42 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index fd9513aab72..a7e54f1d9a0 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -12,3 +12,7 @@ Summary
 
 Important Changes
 -----------------
+
+#### `inverse-kinematics`
+* Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).
+
diff --git a/src/inverse-kinematics/include/iDynTree/InverseKinematics.h b/src/inverse-kinematics/include/iDynTree/InverseKinematics.h
index 1ef18c9ec11..174f19d3610 100644
--- a/src/inverse-kinematics/include/iDynTree/InverseKinematics.h
+++ b/src/inverse-kinematics/include/iDynTree/InverseKinematics.h
@@ -453,6 +453,22 @@ class iDynTree::InverseKinematics
      */
     double getCenterOfMassProjectionMargin();
 
+
+    /*!
+     *
+     * \brief Get the active convex hull
+     *
+     * The convex hull returned is expressed in the plane defined by the xAxisOfPlaneInWorld,
+     * yAxisOfPlaneInWorld and originOfPlaneInWorld arguments of the addCenterOfMassProjectionConstraint methods.
+     *
+     * The transform world_H_constraintFrame that describe how the support polygon for each support frame is transformed
+     * in the world frame are the one set in the addFrameConstraint method.
+     *
+     * @param[out] convexHull constraint convex hull for the projected center of mass.
+     * @return true if the center of mass projection constraint is active, false otherwise.
+     */
+    bool getCenterOfMassProjectConstraintConvexHull(Polygon2D& convexHull);
+
     ///@}
 
     /*! @name Target-related methods
diff --git a/src/inverse-kinematics/src/ConvexHullHelpers.cpp b/src/inverse-kinematics/src/ConvexHullHelpers.cpp
index c3f3be3e06d..d3d01c9cfde 100644
--- a/src/inverse-kinematics/src/ConvexHullHelpers.cpp
+++ b/src/inverse-kinematics/src/ConvexHullHelpers.cpp
@@ -93,6 +93,12 @@ namespace iDynTree
         return m_vertices.size();
     }
 
+    void Polygon2D::setNrOfVertices(size_t size)
+    {
+        m_vertices.resize(size);
+        return;
+    }
+
     Vector2& Polygon2D::operator()(const size_t idx)
     {
         return m_vertices[idx];
diff --git a/src/inverse-kinematics/src/InverseKinematics.cpp b/src/inverse-kinematics/src/InverseKinematics.cpp
index e93f25d402f..7044648027c 100644
--- a/src/inverse-kinematics/src/InverseKinematics.cpp
+++ b/src/inverse-kinematics/src/InverseKinematics.cpp
@@ -394,6 +394,22 @@ namespace iDynTree {
         return IK_PIMPL(m_pimpl)->m_comHullConstraint.computeMargin(comProjection);
     }
 
+    bool InverseKinematics::getCenterOfMassProjectConstraintConvexHull(Polygon2D& poly)
+    {
+        if (!IK_PIMPL(m_pimpl)->m_comHullConstraint.isActive())
+        {
+            poly.setNrOfVertices(0);
+            return false;
+        }
+
+        if (!IK_PIMPL(m_pimpl)->m_problemInitialized) {
+            IK_PIMPL(m_pimpl)->computeProblemSizeAndResizeBuffers();
+        }
+
+        poly = IK_PIMPL(m_pimpl)->m_comHullConstraint.projectedConvexHull;
+        return true;
+    }
+
     bool InverseKinematics::addTarget(const std::string& frameName,
                                       const iDynTree::Transform& constraintValue,
                                       const double positionWeight,

From 1e11137e39203bfd23e638dab23a680aa906371c Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 10 Sep 2018 18:43:34 +0200
Subject: [PATCH 003/194] Add inertial parameters regressors method to
 KinDynComputations

In particular, add the inverseDynamicsInertialParametersRegressor that
computes the (6+nrOfDofs) \times (10*nrOfLinks) matrix  that maps the inertial parameters
to the part of inverse dynamics that depends on inertial parameters,
and the generalizedExternalForces that computes the contribution of external
forces to inverse dynamics (useful because those are not included in the
torques computed by the regressor).
---
 doc/dcTutorialCpp.md                          |  56 +++++----
 .../include/iDynTree/KinDynComputations.h     |  33 +++++
 src/high-level/src/KinDynComputations.cpp     | 116 ++++++++++++++++++
 .../tests/KinDynComputationsUnitTest.cpp      |  34 ++++-
 src/model/include/iDynTree/Model/Dynamics.h   |  54 +++++++-
 src/model/src/Dynamics.cpp                    |  56 +++++++++
 .../integration/DynamicsIntegrationTest.cpp   |  57 +++++++++
 7 files changed, 377 insertions(+), 29 deletions(-)

diff --git a/doc/dcTutorialCpp.md b/doc/dcTutorialCpp.md
index 124d136354e..ca1f9cb2edd 100644
--- a/doc/dcTutorialCpp.md
+++ b/doc/dcTutorialCpp.md
@@ -1,29 +1,36 @@
 
-# DynamicsComputations tutorial
+# KinDynComputations tutorial
 
-To use the DynamicsComputations class, you have to include its header.
+To use the KinDynComputations class, you have to include its header.
 For this tutorial we also use the appropriate `using` definitions to 
-avoid having to type the namespace of the class. 
+avoid having to type the `iDynTree` namespace.
 
 ~~~cpp
 #include <iostream>
 
-#include <iDynTree/HighLevel/DynamicsComputations.h>
+#include <iDynTree/KinDynComputations.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
 
 using namespace iDynTree;
-using namespace iDynTree::HighLevel;
 ~~~
 
 The first thing that we have to do 
 is to initialize the class by providing a model of the robot. Currently 
 iDynTree supports only URDF models to specify the model. 
+All the logic for loading models, including loading reduced models,
+is contained in the iDynTree::ModelLoader class.
 ~~~cpp
 int main()
 {
-    DynamicsComputations dynComp;
-    dynComp.loadRobotModelFromFile("model.urdf");
-    std::cout << "The loaded model has " << dynComp.getNrOfDegreesOfFreedom() 
-          << "internal degrees of freedom and " << dynComp.getNrOfLinks() 
+    ModelLoader mdlLoader;
+    bool ok = mdlLoader.loadModelFromFile(urdf_filename);
+
+    // Check that ok is actually true
+
+    KinDynComputations kinDynComp;
+    kinDynComp.loadRobotModel(mdlLoader.model());
+    std::cout << "The loaded model has " << kinDynComp.model().getNrOfDOFs()
+          << "internal degrees of freedom and " << kinDynComp.model().getNrOfLinks()
           << "links." << std::endl;
 ~~~
 
@@ -34,37 +41,37 @@ to specify the joint position, velocity and accelerations and the gravity in the
 base frame. 
 
 ~~~cpp
-    unsigned int dofs = dynComp.getNrOfDegreesOfFreedom();
-    VectorDynSize q(dofs), dq(dofs), ddq(dofs);
+    unsigned int dofs = kinDynComp.model();
+    VectorDynSize s(dofs), ds(dofs), dds(dofs);
     for(unsigned int dof = 0; dof < dofs; dof++ )
     {
         // For the sake of the example, we fill the joints
         // vector with gibberish data (remember in any case
         // that all quantities are expressed in radians-based 
         // units 
-        q(dof) = 1.0;
-        dq(dof) = 0.4;
-        ddq(dof) = 0.3;
+        s(dof) = 1.0;
+        ds(dof) = 0.4;
+        dds(dof) = 0.3;
     }
     
-    // The spatial acceleration is a 6d acceleration vector. 
+    // The gravity acceleration is a 3d vector.
+    //
     // For all 6d quantities, we use the linear-angular serialization
     // (the first three value are for the linear quantity, the 
     //  the last  three values are for the angular quantity)
-    SpatialAcc gravity;
-    gravity(3) = -9.81;
-    dynComp.setRobotState(q,dq,ddq,gravity);
+    Vector3 gravity;
+    gravity.zero();
+    gravity(2) = -9.81;
+    kinDynComp.setRobotState(s,ds,gravity);
 ~~~
 
 Once you set the state, you can access the computed dynamical quantities. 
 For example you can get the jacobian for a given frame with name "frameName".
 Note that "frameName" should be the name of a link in the URDF model. 
-Note that the jacobian is the floating base jacobian as described in 
-http://wiki.icub.org/codyco/dox/html/dynamics_notation.html . 
 
 ~~~cpp
     MatrixDynSize jac(6,6+dofs);
-    bool ok = dynComp.getFrameJacobian("frameName",jac);
+    bool ok = kinDynComp.getFrameFreeFloatingJacobian("frameName", jac);
     
     if( !ok )
     {
@@ -79,8 +86,7 @@ http://wiki.icub.org/codyco/dox/html/dynamics_notation.html .
 
 You can also get the dynamics regressor. 
 The dynamics regressor is a (6+dofs) \* (10 \* nrOfLinks) Y matrix such that:
-Y pi = M(q) d(v)/dt + C(q,v)v + g(q) 
-with M(q), C(q,v) and g(q) defined in http://wiki.icub.org/codyco/dox/html/dynamics_notation.html .
+Y pi = M(q) d(v)/dt + C(q,v)v + g(q) .
 The pi vector is a 10 \* nrOfLinks inertial parameters vector, such that the elements of the vector
 from the ((i-1) \* 10)-th to the (i \* 10-1)-th belong to the i-th link. For more details on the inertial
 parameters vector, please check https://hal.archives-ouvertes.fr/hal-01137215/document . 
@@ -88,7 +94,9 @@ parameters vector, please check https://hal.archives-ouvertes.fr/hal-01137215/do
 ~~~cpp
     unsigned int links = dynComp.getNrOfLinks();
     MatrixDynSize regr(6+dofs,10*links);
-    ok = dynComp.getDynamicsRegressor(regr);
+    Vector6 baseAcc;
+    baseAcc.zero();
+    ok = kinDynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr);
     
     if( !ok )
     {
diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h
index 1b4bd231be9..6324c2714b7 100644
--- a/src/high-level/include/iDynTree/KinDynComputations.h
+++ b/src/high-level/include/iDynTree/KinDynComputations.h
@@ -697,6 +697,39 @@ class KinDynComputations {
      */
     bool generalizedGravityForces(FreeFloatingGeneralizedTorques & generalizedGravityForces);
 
+    /**
+     * Compute the getNrOfDOFS()+6 vector of generalized external forces.
+     *
+     * The semantics of baseAcc, the base part of baseForceAndJointTorques
+     * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
+     *
+     * The state is the one given set by the setRobotState method.
+     *
+     * @param[out] generalizedExternalForces the output external generalized forces
+     * @return true if all went well, false otherwise
+     */
+    bool generalizedExternalForces(const LinkNetExternalWrenches & linkExtForces,
+                                         FreeFloatingGeneralizedTorques & generalizedExternalForces);
+
+    /**
+     * @brief Compute the free floating inverse dynamics as a linear function of inertial parameters.
+     *
+     * The semantics of baseAcc, the base part (first six rows) of baseForceAndJointTorques
+     * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
+     *
+     * The state is the one given set by the setRobotState method.
+     *
+     * @see iDynTree::InverseDynamicsInertialParametersRegressor for more info on the underlying algorithm.
+     *
+     * @param[in] baseAcc the acceleration of the base link
+     * @param[in] s_ddot the accelerations of the joints
+     * @param[out] baseForceAndJointTorquesRegressor The (6+model.getNrOfDOFs() X 10*model.getNrOfLinks()) inverse dynamics regressor.
+     * @return true if all went well, false otherwise
+     */
+    bool inverseDynamicsInertialParametersRegressor(const Vector6& baseAcc,
+                                                    const VectorDynSize& s_ddot,
+                                                          MatrixDynSize& baseForceAndJointTorquesRegressor);
+
     //@}
 
 
diff --git a/src/high-level/src/KinDynComputations.cpp b/src/high-level/src/KinDynComputations.cpp
index a15d7ed6f34..1b360b0ca41 100644
--- a/src/high-level/src/KinDynComputations.cpp
+++ b/src/high-level/src/KinDynComputations.cpp
@@ -168,6 +168,9 @@ struct KinDynComputations::KinDynComputationsPrivateAttributes
     /** Buffer of link velocities, always set to zero for gravity computations */
     LinkVelArray m_invDynZeroLinkVel;
 
+    /** Buffer of link proper accelerations, always set to zero for external forces */
+    LinkAccArray m_invDynZeroLinkProperAcc;
+
     KinDynComputationsPrivateAttributes()
     {
         m_isModelValid = false;
@@ -242,6 +245,7 @@ void KinDynComputations::resizeInternalDataStructures()
     this->pimpl->m_invDynZeroVel.baseVel().zero();
     this->pimpl->m_invDynZeroVel.jointVel().zero();
     this->pimpl->m_invDynZeroLinkVel.resize(this->pimpl->m_robot_model);
+    this->pimpl->m_invDynZeroLinkProperAcc.resize(this->pimpl->m_robot_model);
     this->pimpl->m_traversalCache.resize(this->pimpl->m_robot_model);
 
     for(LinkIndex lnkIdx = 0; lnkIdx < static_cast<LinkIndex>(pimpl->m_robot_model.getNrOfLinks()); lnkIdx++)
@@ -1909,5 +1913,117 @@ bool KinDynComputations::generalizedGravityForces(FreeFloatingGeneralizedTorques
     return true;
 }
 
+bool KinDynComputations::generalizedExternalForces(const LinkNetExternalWrenches & linkExtForces,
+                                                   FreeFloatingGeneralizedTorques & generalizedExternalForces)
+{
+    // Convert input external forces
+    if( pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION ||
+        pimpl->m_frameVelRepr == MIXED_REPRESENTATION )
+    {
+        this->computeFwdKinematics();
+
+        for(LinkIndex lnkIdx = 0; lnkIdx < static_cast<LinkIndex>(pimpl->m_robot_model.getNrOfLinks()); lnkIdx++)
+        {
+            const Transform & inertialFrame_X_link = pimpl->m_linkPos(lnkIdx);
+            pimpl->m_invDynNetExtWrenches(lnkIdx) = pimpl->fromUsedRepresentationToBodyFixed(linkExtForces(lnkIdx),inertialFrame_X_link);
+        }
+    }
+    else
+    {
+        for(LinkIndex lnkIdx = 0; lnkIdx < static_cast<LinkIndex>(pimpl->m_robot_model.getNrOfLinks()); lnkIdx++)
+        {
+            pimpl->m_invDynNetExtWrenches(lnkIdx) = linkExtForces(lnkIdx);
+        }
+    }
+
+    // Call usual RNEA, but with both velocity and **proper acceleration** set to zero buffers
+    RNEADynamicPhase(pimpl->m_robot_model,
+                     pimpl->m_traversal,
+                     pimpl->m_pos.jointPos(),
+                     pimpl->m_invDynZeroLinkVel,
+                     pimpl->m_invDynZeroLinkProperAcc,
+                     pimpl->m_invDynNetExtWrenches,
+                     pimpl->m_invDynInternalWrenches,
+                     generalizedExternalForces);
+
+    // Convert output base force
+    generalizedExternalForces.baseWrench() = pimpl->fromBodyFixedToUsedRepresentation(generalizedExternalForces.baseWrench(),
+                                                                              pimpl->m_linkPos(pimpl->m_traversal.getBaseLink()->getIndex()));
+    return true;
+}
+
+bool KinDynComputations::inverseDynamicsInertialParametersRegressor(const Vector6& baseAcc,
+                                                                    const VectorDynSize& s_ddot,
+                                                                          MatrixDynSize& regressor)
+{
+    // Needed for using pimpl->m_linkVel
+    this->computeFwdKinematics();
+
+    // Convert input base acceleration
+    if( pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION )
+    {
+        fromEigen(pimpl->m_invDynBaseAcc,toEigen(baseAcc));
+    }
+    else if( pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION )
+    {
+        pimpl->m_invDynBaseAcc = convertInertialAccelerationToBodyFixedAcceleration(baseAcc,pimpl->m_pos.worldBasePos());
+    }
+    else
+    {
+        assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION);
+        pimpl->m_invDynBaseAcc = convertMixedAccelerationToBodyFixedAcceleration(baseAcc,
+                                                                                 pimpl->m_vel.baseVel(),
+                                                                                 pimpl->m_pos.worldBasePos().getRotation());
+    }
+
+    // Prepare the vector of generalized proper accs
+    pimpl->m_invDynGeneralizedProperAccs.baseAcc() = pimpl->m_invDynBaseAcc;
+    toEigen(pimpl->m_invDynGeneralizedProperAccs.baseAcc().getLinearVec3()) =
+        toEigen(pimpl->m_invDynBaseAcc.getLinearVec3()) - toEigen(pimpl->m_gravityAccInBaseLinkFrame);
+    toEigen(pimpl->m_invDynGeneralizedProperAccs.jointAcc()) = toEigen(s_ddot);
+
+    // Run inverse dynamics
+    ForwardAccKinematics(pimpl->m_robot_model,
+                         pimpl->m_traversal,
+                         pimpl->m_pos,
+                         pimpl->m_vel,
+                         pimpl->m_invDynGeneralizedProperAccs,
+                         pimpl->m_linkVel,
+                         pimpl->m_invDynLinkProperAccs);
+
+    // Compute the inverse dynamics regressor, using the absolute frame A as the reference frame in which the base dynamics is expressed
+    // (this is done out of convenience because the pimpl->m_linkPos (that contains for each link L the transform A_H_L) is already available
+    InverseDynamicsInertialParametersRegressor(pimpl->m_robot_model,
+                                               pimpl->m_traversal,
+                                               pimpl->m_linkPos,
+                                               pimpl->m_linkVel,
+                                               pimpl->m_invDynLinkProperAccs,
+                                               regressor);
+
+    // Transform the first six rows of the regressor according to the choosen frame velocity representation
+    if (pimpl->m_frameVelRepr == BODY_FIXED_REPRESENTATION)
+    {
+        int cols = regressor.cols();
+        Matrix6x6 B_X_A = pimpl->m_pos.worldBasePos().inverse().asAdjointTransformWrench();
+        toEigen(regressor).block(0, 0, 6, cols) =
+            toEigen(B_X_A)*toEigen(regressor).block(0, 0, 6, cols);
+
+    }
+    else if (pimpl->m_frameVelRepr == INERTIAL_FIXED_REPRESENTATION)
+    {
+        // In this case, the first six rows are already with the correct value
+    }
+    else
+    {
+        assert(pimpl->m_frameVelRepr == MIXED_REPRESENTATION);
+        int cols = regressor.cols();
+        Matrix6x6 B_A_X_A = Transform(Rotation::Identity(), pimpl->m_pos.worldBasePos().getPosition()).inverse().asAdjointTransformWrench();
+        toEigen(regressor).block(0, 0, 6, cols) =
+            toEigen(B_A_X_A)*toEigen(regressor).block(0, 0, 6, cols);
+    }
+
+    return true;
+}
+
 }
 
diff --git a/src/high-level/tests/KinDynComputationsUnitTest.cpp b/src/high-level/tests/KinDynComputationsUnitTest.cpp
index b9ae465a7de..dd68d30eeb6 100644
--- a/src/high-level/tests/KinDynComputationsUnitTest.cpp
+++ b/src/high-level/tests/KinDynComputationsUnitTest.cpp
@@ -21,6 +21,8 @@
 #include <iDynTree/Core/EigenHelpers.h>
 
 #include <iDynTree/KinDynComputations.h>
+
+#include <iDynTree/Model/Model.h>
 #include <iDynTree/Model/JointState.h>
 #include <iDynTree/Model/FreeFloatingState.h>
 
@@ -180,7 +182,10 @@ void testInverseDynamics(KinDynComputations & dynComp)
     iDynTree::JointDOFsDoubleArray shapeAccs(dynComp.model());
 
     iDynTree::LinkNetExternalWrenches netExternalWrenches(dynComp.model());
-    netExternalWrenches.zero();
+    for(unsigned int link=0; link < dynComp.model().getNrOfLinks(); link++ )
+    {
+        netExternalWrenches(link) = getRandomWrench();
+    }
 
     // Go component for component, for simplifyng debugging
     for(int i=0; i < 6+dofs; i++)
@@ -198,6 +203,7 @@ void testInverseDynamics(KinDynComputations & dynComp)
 
         FreeFloatingGeneralizedTorques invDynForces(dynComp.model());
         FreeFloatingGeneralizedTorques massMatrixInvDynForces(dynComp.model());
+        FreeFloatingGeneralizedTorques regressorInvDynForces(dynComp.model());
 
         // Run classical inverse dynamics
         bool ok = dynComp.inverseDynamics(baseAcc,shapeAccs,netExternalWrenches,invDynForces);
@@ -212,8 +218,12 @@ void testInverseDynamics(KinDynComputations & dynComp)
         ok = dynComp.generalizedBiasForces(invDynBiasForces);
         ASSERT_IS_TRUE(ok);
 
+        FreeFloatingGeneralizedTorques invDynExtForces(dynComp.model());
+        ok = dynComp.generalizedExternalForces(netExternalWrenches, invDynExtForces);
+        ASSERT_IS_TRUE(ok);
+
         VectorDynSize massMatrixInvDynForcesContinuous(6+dofs);
-        toEigen(massMatrixInvDynForcesContinuous) = toEigen(massMatrix)*toEigen(baseAcc,shapeAccs) + toEigen(invDynBiasForces);
+        toEigen(massMatrixInvDynForcesContinuous) = toEigen(massMatrix)*toEigen(baseAcc,shapeAccs) + toEigen(invDynBiasForces) + toEigen(invDynExtForces);
         toEigen(massMatrixInvDynForces.baseWrench().getLinearVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(0);
         toEigen(massMatrixInvDynForces.baseWrench().getAngularVec3()) = toEigen(massMatrixInvDynForcesContinuous).segment<3>(3);
         toEigen(massMatrixInvDynForces.jointTorques()) = toEigen(massMatrixInvDynForcesContinuous).segment(6,dofs);
@@ -221,6 +231,26 @@ void testInverseDynamics(KinDynComputations & dynComp)
         ASSERT_EQUAL_SPATIAL_FORCE(massMatrixInvDynForces.baseWrench(),invDynForces.baseWrench());
         ASSERT_EQUAL_VECTOR(massMatrixInvDynForces.jointTorques(),invDynForces.jointTorques());
 
+        // Run inverse dynamics with regressor matrix
+        MatrixDynSize regressor(dynComp.model().getNrOfDOFs()+6, 10*dynComp.model().getNrOfLinks());
+        ok = dynComp.inverseDynamicsInertialParametersRegressor(baseAcc, shapeAccs, regressor);
+        ASSERT_IS_TRUE(ok);
+
+
+
+        VectorDynSize inertialParams(10*dynComp.model().getNrOfLinks());
+        ok = dynComp.model().getInertialParameters(inertialParams);
+        ASSERT_IS_TRUE(ok);
+
+        VectorDynSize regressorInvDynForcesContinuous(6+dofs);
+        toEigen(regressorInvDynForcesContinuous) = toEigen(regressor)*toEigen(inertialParams) + toEigen(invDynExtForces);
+        toEigen(regressorInvDynForces.baseWrench().getLinearVec3()) = toEigen(regressorInvDynForcesContinuous).segment<3>(0);
+        toEigen(regressorInvDynForces.baseWrench().getAngularVec3()) = toEigen(regressorInvDynForcesContinuous).segment<3>(3);
+        toEigen(regressorInvDynForces.jointTorques()) = toEigen(regressorInvDynForcesContinuous).segment(6,dofs);
+
+        ASSERT_EQUAL_SPATIAL_FORCE(regressorInvDynForces.baseWrench(),invDynForces.baseWrench());
+        ASSERT_EQUAL_VECTOR(regressorInvDynForces.jointTorques(),invDynForces.jointTorques());
+
     }
 }
 
diff --git a/src/model/include/iDynTree/Model/Dynamics.h b/src/model/include/iDynTree/Model/Dynamics.h
index 233b2f93af5..e467c987129 100644
--- a/src/model/include/iDynTree/Model/Dynamics.h
+++ b/src/model/include/iDynTree/Model/Dynamics.h
@@ -11,8 +11,9 @@
 #ifndef IDYNTREE_INVERSE_DYNAMICS_H
 #define IDYNTREE_INVERSE_DYNAMICS_H
 
-#include <iDynTree/Model/Indices.h>
+#include <iDynTree/Core/MatrixDynSize.h>
 
+#include <iDynTree/Model/Indices.h>
 #include <iDynTree/Model/LinkState.h>
 #include <iDynTree/Model/JointState.h>
 
@@ -60,12 +61,26 @@ namespace iDynTree
                                                        const LinkAccArray & linkBiasAcc,
                                                              Wrench& totalMomentumBias);
 
-
+    /**
+     * \ingroup iDynTreeModel
+     *
+     * @brief Compute the inverse dynamics, i.e. the generalized torques corresponding to a given set of robot accelerations and external force/torques.
+     *
+     * @param[in] model The model used for the computation.
+     * @param[in] traversal The traversal used for the computation, it defines the used base link.
+     * @param[in] jointPos The (internal) joint position of the model.
+     * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$).
+     * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model
+     *                           (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ . See iDynTree::LinkNetExternalWrenches .
+     * @param[in] linkExtForces Vector of external 6D force/torques applied to the links. For each link \f$L\f$, the corresponding external force is \f${}_L \mathrm{f}^x_L\f$, i.e. the force that the enviroment applies on the on the link \f$L\f$, expressed in the link frame \f$L\f$.
+     * @param[out] linkIntWrenches Vector of internal joint force/torques. See iDynTree::LinkInternalWrenches .
+     * @param[out] baseForceAndJointTorques Generalized torques output. The base element is the residual force on the base (that is equal to zero if the robot acceleration and the external forces provided in LinkNetExternalWrenches were consistent), while the joint part is composed by the joint torques.
+     */
     bool RNEADynamicPhase(const iDynTree::Model & model,
                           const iDynTree::Traversal & traversal,
                           const iDynTree::JointPosDoubleArray & jointPos,
                           const iDynTree::LinkVelArray & linksVel,
-                          const iDynTree::LinkAccArray & linksAcc,
+                          const iDynTree::LinkAccArray & linksProperAcc,
                           const iDynTree::LinkNetExternalWrenches & linkExtForces,
                                 iDynTree::LinkInternalWrenches       & linkIntWrenches,
                                 iDynTree::FreeFloatingGeneralizedTorques & baseForceAndJointTorques);
@@ -128,6 +143,8 @@ namespace iDynTree
     };
 
     /**
+     * \ingroup iDynTreeModel
+     *
      * Compute the floating base acceleration of an unconstrianed
      * robot, using as input the external forces and the joint torques.
      * We follow the algorithm described in Featherstone 2008, modified
@@ -143,6 +160,37 @@ namespace iDynTree
                                         ArticulatedBodyAlgorithmInternalBuffers & buffers,
                                         FreeFloatingAcc & robotAcc);
 
+    /**
+     * \ingroup iDynTreeModel
+     *
+     *
+     * @brief Compute the inverse dynamics of the model as linear function of the inertial parameters.
+     *
+     * This function computes the matrix that multiplied by the vector of inertial parameters of a model (see iDynTree::Model::getInertialParameters)
+     * returns the inverse dynamics generalized torques. In particular it is consistent with the result of the iDynTree::RNEADynamicPhase function, i.e.
+     * the first six rows of the regressor correspond to the sum of all external force/torques acting on the robot, expressed in the origin
+     * and with the orientation of the specified referenceFrame, as defined by the referenceFrame_H_link argument.
+     *
+     *
+     * @note The regressor only computes the inverse dynamics generalized torques assuming that the external forces are equal to zero,
+     *       as the contribution of the external forces to the inverse dynamics is indipendent from inertial parameters.
+     *
+     * @param[in] model The model used for the computation.
+     * @param[in] traversal The traversal used for the computation, it defines the used base link.
+     * @param[in] referenceFrame_H_link Position of  each link w.r.t. to given frame D (tipically an inertial frame A, the base frame B or the mixed frame B[A]). For each link \f$L\f$, the corresponding transform is \f${}^D H_L\f$.
+     * @param[in] linksVel Vector of left-trivialized velocities for each link of the model (for each link \f$L\f$, the corresponding velocity is \f${}^L \mathrm{v}_{A, L}\f$).
+     * @param[in] linksProperAcc Vector of left-trivialized proper acceleration for each link of the model
+     *                           (for each link \f$L\f$, the corresponding proper acceleration is \f${}^L \dot{\mathrm{v}}_{A, L} - \begin{bmatrix} {}^L R_A {}^A g \\ 0_{3\times1} \end{bmatrix} \f$), where \f$ {}^A g \in \mathbb{R}^3 \f$ is the gravity acceleration expressed in an inertial frame \f$A\f$ .
+     * @param[out] baseForceAndJointTorquesRegressor The (6+model.getNrOfDOFs() X 10*model.getNrOfLinks()) inverse dynamics regressor.
+     *
+     */
+    bool InverseDynamicsInertialParametersRegressor(const iDynTree::Model & model,
+                                                    const iDynTree::Traversal & traversal,
+                                                    const iDynTree::LinkPositions& referenceFrame_H_link,
+                                                    const iDynTree::LinkVelArray & linksVel,
+                                                    const iDynTree::LinkAccArray & linksAcc,
+                                                          iDynTree::MatrixDynSize & baseForceAndJointTorquesRegressor);
+
 
 
 }
diff --git a/src/model/src/Dynamics.cpp b/src/model/src/Dynamics.cpp
index d9d0d1fa081..53cacb98075 100644
--- a/src/model/src/Dynamics.cpp
+++ b/src/model/src/Dynamics.cpp
@@ -19,6 +19,7 @@
 #include <iDynTree/Model/JointState.h>
 
 #include <iDynTree/Core/ArticulatedBodyInertia.h>
+#include <iDynTree/Core/SpatialInertia.h>
 #include <iDynTree/Core/SpatialMomentum.h>
 #include <iDynTree/Core/EigenHelpers.h>
 
@@ -476,6 +477,61 @@ bool ArticulatedBodyAlgorithm(const Model& model,
     return true;
 }
 
+bool InverseDynamicsInertialParametersRegressor(const iDynTree::Model & model,
+                                                const iDynTree::Traversal & traversal,
+                                                const iDynTree::LinkPositions& referenceFrame_H_link,
+                                                const iDynTree::LinkVelArray & linksVel,
+                                                const iDynTree::LinkAccArray & linksProperAcc,
+                                                      iDynTree::MatrixDynSize & baseForceAndJointTorquesRegressor)
+{
+    baseForceAndJointTorquesRegressor.resize(6+model.getNrOfDOFs(), 10*model.getNrOfLinks());
+    baseForceAndJointTorquesRegressor.zero();
+
+    // Eigen map of the input matrix
+    iDynTreeEigenMatrixMap regressor = iDynTree::toEigen(baseForceAndJointTorquesRegressor);
+
+    Matrix6x10 netForceTorqueRegressor_i;
+
+    for (TraversalIndex l =(TraversalIndex)traversal.getNrOfVisitedLinks()-1; l >= 0; l-- ) {
+
+        // In this cycle, we compute the contribution of the inertial parameters of link to the inverse dynamics results
+        LinkConstPtr link = traversal.getLink(l);
+        LinkIndex lnkIdx = link->getIndex();
+
+        // Each link affects the dynamics of the joints from itself to the base
+        netForceTorqueRegressor_i = SpatialInertia::momentumDerivativeRegressor(linksVel(lnkIdx),
+                                                                                linksProperAcc(lnkIdx));
+
+        iDynTreeEigenMatrix linkRegressor = iDynTree::toEigen(netForceTorqueRegressor_i);
+
+        // Base dynamics
+        // The base dynamics is expressed with the orientation of the base and with respect to the base origin
+        regressor.block(0,(int)(10*lnkIdx),6,10) =
+            toEigen(referenceFrame_H_link(lnkIdx).asAdjointTransformWrench())*linkRegressor;
+
+        LinkIndex visitedLinkIdx  = lnkIdx;
+
+        while (visitedLinkIdx != traversal.getBaseLink()->getIndex())
+        {
+            LinkIndex parentLinkIdx = traversal.getParentLinkFromLinkIndex(visitedLinkIdx)->getIndex();
+            IJointConstPtr joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx);
+
+            size_t dofOffset = joint->getDOFsOffset();
+            for(int i=0; i < joint->getNrOfDOFs(); i++)
+            {
+                iDynTree::SpatialMotionVector S = joint->getMotionSubspaceVector(i,visitedLinkIdx,parentLinkIdx);
+                Transform visitedLink_H_link = referenceFrame_H_link(visitedLinkIdx).inverse()*referenceFrame_H_link(lnkIdx);
+                regressor.block(6+dofOffset+i,10*lnkIdx,1,10) =
+                    toEigen(S).transpose()*(toEigen(visitedLink_H_link.asAdjointTransformWrench())*linkRegressor);
+            }
+
+            visitedLinkIdx = parentLinkIdx;
+        }
+
+    }
+
+    return true;
+}
 
 
 }
diff --git a/src/tests/integration/DynamicsIntegrationTest.cpp b/src/tests/integration/DynamicsIntegrationTest.cpp
index dc52f9155d8..f6a0d8774a8 100644
--- a/src/tests/integration/DynamicsIntegrationTest.cpp
+++ b/src/tests/integration/DynamicsIntegrationTest.cpp
@@ -9,6 +9,7 @@
  */
 
 
+#include <iDynTree/Core/EigenHelpers.h>
 #include <iDynTree/Core/TestUtils.h>
 
 #include <iDynTree/Model/Model.h>
@@ -109,6 +110,62 @@ void checkInverseAndForwardDynamicsAreIdempotent(const Model & model,
                         zeroVec,1e-6);
     std::cout << "Check joint torques" << std::endl;
     ASSERT_EQUAL_VECTOR_TOL(RNEA_baseForceAndJointTorques.jointTorques(),ABA_jntTorques,1e-07);
+
+    // Also check that the inverse dynamics as computed by regressor*inertial parameters  is consistent
+    MatrixDynSize regressor(6+model.getNrOfDOFs(), 10*model.getNrOfLinks());
+    VectorDynSize inertialParams(10*model.getNrOfLinks());
+    VectorDynSize invDynResults(6+model.getNrOfDOFs());
+
+    bool ok = model.getInertialParameters(inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    // For computing inertial params we need the transform between baseLink and each link
+    LinkPositions baseLink_H_link(model);
+    ok = ForwardPositionKinematics(model,
+                                   traversal,
+                                   Transform::Identity(),
+                                   robotPos.jointPos(),
+                                   baseLink_H_link);
+    ASSERT_IS_TRUE(ok);
+
+    ok = InverseDynamicsInertialParametersRegressor(model,
+                                                    traversal,
+                                                    baseLink_H_link,
+                                                    RNEA_linksVel,
+                                                    RNEA_linksAcc,
+                                                    regressor);
+
+    // The regressor just accounts for the gravitational and "inertial" forces, but we should
+    // take into account also the effect of external forces
+    // Allocate temporary data structure for RNEA for computing ext forces
+    LinkVelArray RNEA_EXT_linksVel(model); // Zero
+    LinkAccArray RNEA_EXT_linksAcc(model); // Zero
+    LinkInternalWrenches RNEA_EXT_linkIntWrenches(model);
+    FreeFloatingGeneralizedTorques RNEA_EXT_baseForceAndJointTorques(model);
+
+    RNEADynamicPhase(model,
+                     traversal,
+                     robotPos.jointPos(),
+                     RNEA_EXT_linksVel,
+                     RNEA_EXT_linksAcc,
+                     linkExtWrenches,
+                     RNEA_EXT_linkIntWrenches,
+                     RNEA_EXT_baseForceAndJointTorques);
+
+    ASSERT_IS_TRUE(ok);
+
+    toEigen(invDynResults) =
+        toEigen(regressor)*toEigen(inertialParams);
+
+    Vector6 REGR_baseWrench;
+    VectorDynSize REGR_jointTorques(model.getNrOfDOFs());
+
+    toEigen(REGR_baseWrench) = toEigen(invDynResults).segment<6>(0) + toEigen(RNEA_EXT_baseForceAndJointTorques.baseWrench());
+    toEigen(REGR_jointTorques) = toEigen(invDynResults).segment(6, model.getNrOfDOFs()) + toEigen(RNEA_EXT_baseForceAndJointTorques.jointTorques());
+
+    double tolRegr = 1e-6;
+    ASSERT_EQUAL_VECTOR_TOL(REGR_baseWrench, RNEA_baseForceAndJointTorques.baseWrench().asVector(), tolRegr);
+    ASSERT_EQUAL_VECTOR_TOL(REGR_jointTorques, RNEA_baseForceAndJointTorques.jointTorques(), tolRegr);
 }
 
 int main()

From 9cb82d60bfe712107b7f50e036ec937241053545 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 11 Sep 2018 13:13:37 +0200
Subject: [PATCH 004/194] Add documentation for
 inverseDynamicsRegressorInertialParameters

Cleanup also documentation for KinDynComputations, to simplify the
process of adding docs for the new method.
---
 .../include/iDynTree/KinDynComputations.h     | 127 +++++++++++-------
 .../iDynTree/Model/FreeFloatingMatrices.h     |  10 +-
 2 files changed, 85 insertions(+), 52 deletions(-)

diff --git a/src/high-level/include/iDynTree/KinDynComputations.h b/src/high-level/include/iDynTree/KinDynComputations.h
index 6324c2714b7..83ad6bb2b1b 100644
--- a/src/high-level/include/iDynTree/KinDynComputations.h
+++ b/src/high-level/include/iDynTree/KinDynComputations.h
@@ -108,6 +108,7 @@ class KinDynComputations {
     /**
      * @name Model loading and definition methods
      * This methods are used to load the structure of your model.
+     *
      */
     //@{
 
@@ -148,11 +149,14 @@ class KinDynComputations {
 
     /**
      * Set the used FrameVelocityRepresentation.
+     *
+     * @see FrameVelocityRepresentation
      */
     bool setFrameVelocityRepresentation(const FrameVelocityRepresentation frameVelRepr) const;
 
     /**
-     * Get the used FrameVelocityRepresentation.
+     * @brief Get the used FrameVelocityRepresentation.
+     * @see setFrameVelocityRepresentation
      */
     FrameVelocityRepresentation getFrameVelocityRepresentation() const;
     //@}
@@ -188,20 +192,6 @@ class KinDynComputations {
      */
     unsigned int getNrOfLinks() const;
 
-    /**
-     * Get a human readable description of a given link in the model.
-     *
-     * @return a human readable description of a given link in the model.
-     */
-    //std::string getDescriptionOfLink(int link_index);
-
-    /**
-     * Get a human readable description of all links considered in the model.
-     *
-     * @return a std::string containing the description of all the links.
-     */
-    //std::string getDescriptionOfLinks();
-
     /**
      * Get the number of frames contained in the model.
      *
@@ -212,20 +202,6 @@ class KinDynComputations {
      */
     unsigned int getNrOfFrames() const;
 
-    /**
-     * Get a human readable description of a given frame considered in the model.
-     *
-     * @return a human readable description of a given frame considered in the model.
-     */
-    //std::string getDescriptionOfFrame(int frame_index);
-
-    /**
-     * Get a human readable description of all frames considered in the model.
-     *
-     * @return a std::string containing the description of all the frame considered in the model.
-     */
-    //std::string getDescriptionOfLinks();
-
     /**
      * Get the name of the link considered as the floating base.
      *
@@ -368,8 +344,7 @@ class KinDynComputations {
     //@}
 
     /**
-      * @name Methods to get transform information between frames in the model,
-      *       given the current state.
+      * @name Methods to get transform information between frames in the model, given the current state.
       */
     //@{
 
@@ -623,12 +598,54 @@ class KinDynComputations {
 
 
     /**
-      * @name Methods to get quantities related to dynamics matrices.
+      * @name Methods to get quantities related to unconstrained free floating equation of motions.
+      *
+      * This methods permits to compute several quantities related to free floating equation of methods.
+      * Note that this equations needs to be coupled with a description of the interaction between the model
+      * and the enviroment (such as a contant model, a bilateral constraint on some links or by considering
+      * some external forces as inputs) to actually obtain a dynamical system description of the mechanical model evolution.
+      *
+      * The equations of motion of a free floating mechanical system under the effect of a uniform gravitational field are:
+      * \f[
+      * M(q) \dot{\nu} +
+      * C(q, \nu) \nu +
+      * G(q)
+      * =
+      * \begin{bmatrix}
+      * 0_{6\times1} \newline
+      * \tau
+      * \end{bmatrix}
+      * +
+      * \sum_{L \in \mathcal{L}}
+      * J_L^T \mathrm{f}_L^x
+      * \f]
+      *
+      * where:
+      *
+      * * \f$n_{PC}\f$ is the value returned by Model::getNrOfPosCoords,
+      * * \f$n_{DOF}\f$ is the value returned by Model::getNrOfDOFs,
+      * * \f$n_{L}\f$ is the value returned by Model::getNrOfLinks,
+      * * \f$q \in \mathbb{R}^3 \times \textrm{SO}(3) \times \mathbb{R}^{n_{PC}}\f$ is the robot position,
+      * * \f$\nu \in \mathbb{R}^{6+n_{DOF}}\f$ is the robot velocity,
+      * * \f$\dot{\nu} \in \mathbb{R}^{6+n_{DOF}}\f$ is the robot acceleration,
+      * * \f$M(q) \in \mathbb{R}^{(6+n_{DOF}) \times (6+n_{DOF})}\f$ is the free floating mass matrix,
+      * * \f$C(q, \nu)  \in \mathbb{R}^{(6+n_{DOF}) \times (6+n_{DOF})}\f$ is the coriolis matrix,
+      * * \f$G(q) \in \mathbb{R}^{6+n_{DOF}}\f$ is the vector of gravity generalized forces,
+      * * \f$\tau \in \mathbb{R}^6\f$ is the vector of torques applied on the joint of the multibody model,
+      * * \f$\mathcal{L}\f$ is the set of all the links contained in the multibody model,
+      * * \f$J_L \in \mathbb{R}^{6+n_{DOF}}\f$ is the free floating jacobian of link \f$L\f$ as obtained by KinDynComputations::getFrameFreeFloatingJacobian,
+      * * \f$\mathrm{f}_L^x\f$ is the 6D force/torque applied by the enviroment on link \f$L\f$.
+      *
+      * The precise definition of each quantity (in particular the part related to the base) actually depends on the
+      * choice of FrameVelocityRepresentation, specified with the setFrameVelocityRepresentation method.
+      *
       */
     //@{
 
     /**
-     * Get the free floating mass matrix of the system.
+     * @brief Get the free floating mass matrix of the system.
+     *
+     * This method computes \f$M(q) \in \mathbb{R}^{(6+n_{DOF}) \times (6+n_{DOF})}\f$.
      *
      * The mass matrix depends on the joint positions, specified by the setRobotState methods.
      * If the chosen FrameVelocityRepresentation is MIXED_REPRESENTATION or INERTIAL_FIXED_REPRESENTATION,
@@ -645,15 +662,11 @@ class KinDynComputations {
      */
     bool getFreeFloatingMassMatrix(MatrixDynSize & freeFloatingMassMatrix);
 
-    //@}
 
     /**
-      * @name Methods to unconstrained free floating dynamics.
-      */
-    //@{
-
-    /**
-     * Compute the free floating inverse dynamics.
+     * @brief Compute the free floating inverse dynamics.
+     *
+     * This method computes \f$M(q) \dot{\nu} + C(q, \nu) \nu + G(q) - \sum_{L \in \mathcal{L}} J_L^T \mathrm{f}_L^x \in \mathbb{R}^{6+n_{DOF}}\f$.
      *
      * The semantics of baseAcc, the base part of baseForceAndJointTorques
      * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
@@ -672,10 +685,11 @@ class KinDynComputations {
                                FreeFloatingGeneralizedTorques & baseForceAndJointTorques);
 
     /**
-     * Compute the getNrOfDOFS()+6 vector of generalized bias (gravity+coriolis) forces.
+     * @brief Compute the getNrOfDOFS()+6 vector of generalized bias (gravity+coriolis) forces.
      *
-     * The semantics of baseAcc, the base part of baseForceAndJointTorques
-     * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
+     * This method computes \f$C(q, \nu) \nu + G(q) \in \mathbb{R}^{6+n_{DOF}}\f$.
+     *
+     * The semantics of the base part of generalizedBiasForces depend of the chosen FrameVelocityRepresentation .
      *
      * The state is the one given set by the setRobotState method.
      *
@@ -685,10 +699,11 @@ class KinDynComputations {
     bool generalizedBiasForces(FreeFloatingGeneralizedTorques & generalizedBiasForces);
 
     /**
-     * Compute the getNrOfDOFS()+6 vector of generalized gravity forces.
+     * @brief Compute the getNrOfDOFS()+6 vector of generalized gravity forces.
      *
-     * The semantics of baseAcc, the base part of baseForceAndJointTorques
-     * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
+     * This method computes \f$G(q) \in \mathbb{R}^{6+n_{DOF}}\f$.
+     *
+     * The semantics of the base part of generalizedGravityForces depend of the chosen FrameVelocityRepresentation .
      *
      * The state is the one given set by the setRobotState method.
      *
@@ -698,9 +713,14 @@ class KinDynComputations {
     bool generalizedGravityForces(FreeFloatingGeneralizedTorques & generalizedGravityForces);
 
     /**
-     * Compute the getNrOfDOFS()+6 vector of generalized external forces.
+     * @brief Compute the getNrOfDOFS()+6 vector of generalized external forces.
      *
-     * The semantics of baseAcc, the base part of baseForceAndJointTorques
+     * This method computes \f$ -\sum_{L \in \mathcal{L}} J_L^T \mathrm{f}_L^x \in \mathbb{R}^{6+n_{DOF}} \f$.
+     *
+     * @warning Note that this method returns the **negated** sum of the product of jacobian and the external force,
+     *          consistently with how the generalized external forces are computed in the KinDynComputations::inverseDynamics method.
+     *
+     * The semantics of the base part of generalizedExternalForces
      * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
      *
      * The state is the one given set by the setRobotState method.
@@ -714,8 +734,15 @@ class KinDynComputations {
     /**
      * @brief Compute the free floating inverse dynamics as a linear function of inertial parameters.
      *
-     * The semantics of baseAcc, the base part (first six rows) of baseForceAndJointTorques
-     * and of the elements of linkExtWrenches depend of the chosen FrameVelocityRepresentation .
+     * This methods computes the \f$ Y(\dot{\nu}, \nu, q) \in \mathbb{R}^{ (6+n_{DOF}) \times (10n_{L}) } \f$ matrix such that:
+     * \f[
+     *  Y(\dot{\nu}, \nu, q) \phi = M(q) \dot{\nu} + C(q, \nu) \nu + G(q)
+     * \f]
+     *
+     * where \f$\phi \in \mathbb{R}^{10n_{L}}\f$ is the vector of inertial parameters returned by the Model::getInertialParameters .
+     *
+     * The semantics of baseAcc, the base part (first six rows) of baseForceAndJointTorquesRegressor
+     * depend of the chosen FrameVelocityRepresentation .
      *
      * The state is the one given set by the setRobotState method.
      *
diff --git a/src/model/include/iDynTree/Model/FreeFloatingMatrices.h b/src/model/include/iDynTree/Model/FreeFloatingMatrices.h
index 6404629b140..d51c1ac507e 100644
--- a/src/model/include/iDynTree/Model/FreeFloatingMatrices.h
+++ b/src/model/include/iDynTree/Model/FreeFloatingMatrices.h
@@ -19,9 +19,15 @@ namespace iDynTree
 class Model;
 
 /**
- * Enum describing the possible frame velocity representation convention.
+ * @brief Possible frame velocity representation convention.
  *
- * See KinDynComputations documentation for more details.
+ * Given a link \f$L\f$ and an absolute frame \f$A\f$, the
+ * the possible frame velocity representation are the following:
+ * * `INERTIAL_FIXED_REPRESENTATION` : Velocity representation is \f${}^{A} \mathrm{v}_{A,B}\f$,
+ * * `BODY_FIXED_REPRESENTATION` : Velocity representation is \f${}^{B} \mathrm{v}_{A,B}\f$,
+ * * `MIXED_REPRESENTATION` : Velocity representation is \f${}^{B[A]} \mathrm{v}_{A,B}\f$.
+ *
+ * See iDynTree::KinDynComputations documentation for more details.
  */
 enum FrameVelocityRepresentation
 {

From 2d6a48d6c11163ebed7bfde413928d773855cd74 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 11 Sep 2018 14:02:22 +0200
Subject: [PATCH 005/194] Migrate python from DynamicsComputations to
 KinDynComputations

---
 .../python/dynamicsComputationTutorial.py     | 50 +++++++++++--------
 1 file changed, 28 insertions(+), 22 deletions(-)

diff --git a/examples/python/dynamicsComputationTutorial.py b/examples/python/dynamicsComputationTutorial.py
index 7b6719bc773..dcfbcdb7990 100644
--- a/examples/python/dynamicsComputationTutorial.py
+++ b/examples/python/dynamicsComputationTutorial.py
@@ -6,46 +6,52 @@
 """
 
 import iDynTree
-from iDynTree import DynamicsComputations
+from iDynTree import KinDynComputations
+from iDynTree import ModelLoader
+
 
 URDF_FILE = '/home/username/path/robot.urdf';
 
-dynComp = DynamicsComputations();
-dynComp.loadRobotModelFromFile(URDF_FILE);
-print "The loaded model has", dynComp.getNrOfDegreesOfFreedom(), \
-    "internal degrees of freedom and",dynComp.getNrOfLinks(),"links."
+dynComp = KinDynComputations();
+mdlLoader = ModelLoader();
+mdlLoader.loadModelFromFile(URDF_FILE);
+
+dynComp.loadRobotModel(mdlLoader.model());
+
+print "The loaded model has", dynComp.model().getNrOfDOFs(), \
+    "internal degrees of freedom and",dynComp.model().getNrOfLinks(),"links."
 
-dofs = dynComp.getNrOfDegreesOfFreedom();
-q = iDynTree.VectorDynSize(dofs);
-dq = iDynTree.VectorDynSize(dofs);
-ddq = iDynTree.VectorDynSize(dofs);
+dofs = dynComp.model().getNrOfDOFs();
+s = iDynTree.VectorDynSize(dofs);
+ds = iDynTree.VectorDynSize(dofs);
+dds = iDynTree.VectorDynSize(dofs);
 for dof in range(dofs):
     # For the sake of the example, we fill the joints vector with gibberish data (remember in any case
     # that all quantities are expressed in radians-based units 
-    q.setVal(dof, 1.0);
-    dq.setVal(dof, 0.4);
-    ddq.setVal(dof, 0.3);
+    s.setVal(dof, 1.0);
+    ds.setVal(dof, 0.4);
+    dds.setVal(dof, 0.3);
 
 
-# The spatial acceleration is a 6d acceleration vector. 
-# For all 6d quantities, we use the linear-angular serialization
-# (the first three value are for the linear quantity, the 
-#  the last  three values are for the angular quantity)
-gravity = iDynTree.SpatialAcc();
+# The gravity acceleration is a 3d acceleration vector.
+gravity = iDynTree.Vector3();
 gravity.zero();
 gravity.setVal(2, -9.81);
-dynComp.setRobotState(q,dq,ddq,gravity);
+dynComp.setRobotState(s,ds,gravity);
 
 jac = iDynTree.MatrixDynSize(6,6+dofs);
-ok = dynComp.getFrameJacobian("lf_foot", jac);
+ok = dynComp.getFreeFloatingJacobian("lf_foot", jac);
 if( not ok ):
     print "Error in computing jacobian of frame " + "lf_foot";
 else: 
     print "Jacobian of lf_foot is\n" + jac.toString();
-    
-links = dynComp.getNrOfLinks();
+
+
+baseAcc = iDynTree.Vector6();
+baseAcc.zero();
+links = dynComp.model().getNrOfLinks();
 regr = iDynTree.MatrixDynSize(6+dofs,10*links);
-ok = dynComp.getDynamicsRegressor(regr);
+ok = dynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr);
 if( not ok ):
     print "Error in computing the dynamics regressor";
 else :

From c0403ead4a3b471f336a5d26d457089caaf0f339 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 11 Sep 2018 14:02:39 +0200
Subject: [PATCH 006/194] Cleanup of deprecated classes

---
 doc/main.dox                                        | 13 +++++++++++++
 .../iDynTree/Estimation/simpleLeggedOdometryKDL.h   |  2 +-
 .../iDynTree/HighLevel/DynamicsComputations.h       |  2 +-
 3 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/doc/main.dox b/doc/main.dox
index 81ee58e68bb..b37d9695ce3 100644
--- a/doc/main.dox
+++ b/doc/main.dox
@@ -83,6 +83,13 @@
  */
 
 
+ /**
+ * \defgroup iDynTreeDeprecated Deprecated functions and classes
+ *
+ * \warning The functions and classes in this component should not be used for new code.
+ *
+ */
+
 
 
 
@@ -121,5 +128,11 @@ Until this components are ready to be integrated in a proper part of iDynTree, w
 
 \li  \ref iDynTreeExperimental "Experimental" : Experimental data structures and algorithms, whose interface is not guaranteed to be stable.
 
+iDynTree started as a tiny wrapper of code between [YARP](http://yarp.it/) and [KDL](https://github.com/orocos/orocos_kinematics_dynamics).
+For this reason, in the repository there is still some legacy code, that should not be used by users, as it is going to be removed from iDynTree.
+
+\li  \ref iDynTreeDeprecated "Deprecated" : Deprecated data structures and algorithms, that are going to be removed from iDynTree.
+
+
  *
  */
diff --git a/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h b/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h
index 5391ccf78f6..0933233da5b 100644
--- a/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h
+++ b/src/legacy/estimation-kdl/include/iDynTree/Estimation/simpleLeggedOdometryKDL.h
@@ -15,7 +15,7 @@
 
 namespace iDynTree {
 /**
- * \ingroup iDynTreeEstimation
+ * \ingroup iDynTreeDeprecated
  *
  * A simple legged odometry for a legged robot.
  *
diff --git a/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h b/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h
index cc4625e173f..a4378c4ee07 100644
--- a/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h
+++ b/src/legacy/high-level-kdl/include/iDynTree/HighLevel/DynamicsComputations.h
@@ -30,7 +30,7 @@ class Wrench;
 namespace HighLevel {
 
 /**
- * \ingroup iDynTreeHighLevel
+ * \ingroup iDynTreeDeprecated
  *
  * The dynamics computations class is an high level class stateful to access
  * several algorithms related to kinematics and dynamics of free floating

From 122beec6b9f7d06c976065262f36dee32490b505 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 11 Sep 2018 14:40:43 +0200
Subject: [PATCH 007/194] Update changelog

---
 doc/releases/v0_12.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index a7e54f1d9a0..5447674a41e 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -13,6 +13,10 @@ Summary
 Important Changes
 -----------------
 
+#### `high-level`
+* Added method to compute the inverse dynamics inertial parameters regressor in KinDynComputations ( https://github.com/robotology/idyntree/pull/480 ).
+KinDynComputations finally reached feature parity with respect to DynamicsComputations, that will finally be removed in one of the future iDynTree feature releases.
+
 #### `inverse-kinematics`
 * Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).
 

From 08ca5aec2a8f4d9a60d77648d8584e7edfd97b41 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 31 May 2018 18:31:14 +0200
Subject: [PATCH 008/194] Added info structure for the OptimizationProblem.

---
 .../include/iDynTree/Integrator.h             |  4 +-
 .../include/iDynTree/OptimizationProblem.h    | 55 ++++++++++++++++
 .../src/OptimizationProblem.cpp               | 62 +++++++++++++++++++
 3 files changed, 119 insertions(+), 2 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index 50badf4f97b..5064ef55d76 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -67,9 +67,9 @@ namespace optimalcontrol {
 
                 const std::string &name() const;
 
-                 bool isExplicit() const;
+                bool isExplicit() const;
 
-                 size_t numberOfStages() const;
+                size_t numberOfStages() const;
             };
 
             /**
diff --git a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
index fc26d328388..066a83c602a 100644
--- a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
@@ -18,6 +18,7 @@
 #define IDYNTREE_OPTIMALCONTROL_OPTIMIZATIONPROBLEM_H
 
 #include <vector>
+#include <memory>
 #include <cstddef>
 
 namespace iDynTree {
@@ -33,6 +34,55 @@ namespace iDynTree {
          * \ingroup iDynTreeExperimental
          */
 
+        class OptimizationProblemInfoData {
+        protected:
+            friend class OptimizationProblem;
+            OptimizationProblemInfoData();
+        public:
+            bool hasLinearConstraints;
+
+            bool hasNonLinearConstraints;
+
+            bool hasLinearCost;
+
+            bool hasQuadraticCost;
+
+            bool hasNonLinearCost;
+
+            bool hasSparseConstraintJacobian;
+
+            bool hasSparseHessian;
+
+            bool hessianIsProvided;
+        };
+
+        class OptimizationProblemInfo {
+        private:
+            std::shared_ptr<OptimizationProblemInfoData> m_data;
+        public:
+            OptimizationProblemInfo(std::shared_ptr<OptimizationProblemInfoData> data);
+
+            OptimizationProblemInfo() = delete;
+
+            OptimizationProblemInfo(const OptimizationProblemInfo &other) = delete;
+
+            bool hasLinearConstraints() const;
+
+            bool hasNonLinearConstraints() const;
+
+            bool hasLinearCost() const;
+
+            bool hasQuadraticCost() const;
+
+            bool hasNonLinearCost() const;
+
+            bool hasSparseConstraintJacobian() const;
+
+            bool hasSparseHessian() const;
+
+            bool hessianIsProvided() const;
+        };
+
         class OptimizationProblem {
 
         public:
@@ -73,6 +123,11 @@ namespace iDynTree {
 
             virtual bool evaluateConstraintsHessian(const VectorDynSize& constraintsMultipliers, MatrixDynSize& hessian); //using dense matrices, but the sparsity pattern is still obtained
 
+            const OptimizationProblemInfo& info() const;
+
+        protected:
+            std::shared_ptr<OptimizationProblemInfoData> m_infoData;
+            OptimizationProblemInfo m_info;
         };
     }
 }
diff --git a/src/optimalcontrol/src/OptimizationProblem.cpp b/src/optimalcontrol/src/OptimizationProblem.cpp
index a809c26bf67..b66698c2ccc 100644
--- a/src/optimalcontrol/src/OptimizationProblem.cpp
+++ b/src/optimalcontrol/src/OptimizationProblem.cpp
@@ -22,6 +22,8 @@ namespace iDynTree {
     namespace optimization {
 
         OptimizationProblem::OptimizationProblem()
+        :m_infoData(new OptimizationProblemInfoData)
+        ,m_info(m_infoData)
         {
         }
 
@@ -112,5 +114,65 @@ namespace iDynTree {
             return false;
         }
 
+        const OptimizationProblemInfo &OptimizationProblem::info() const
+        {
+            return m_info;
+        }
+
+        OptimizationProblemInfoData::OptimizationProblemInfoData()
+        : hasLinearConstraints(false)
+        , hasNonLinearConstraints(true)
+        , hasLinearCost(false)
+        , hasQuadraticCost(false)
+        , hasNonLinearCost(true)
+        , hasSparseConstraintJacobian(false)
+        , hasSparseHessian(false)
+        , hessianIsProvided(false)
+        { }
+
+        OptimizationProblemInfo::OptimizationProblemInfo(std::shared_ptr<OptimizationProblemInfoData> data)
+            : m_data(data)
+        { }
+
+        bool OptimizationProblemInfo::hasLinearConstraints() const
+        {
+            return m_data->hasLinearConstraints;
+        }
+
+        bool OptimizationProblemInfo::hasNonLinearConstraints() const
+        {
+            return m_data->hasNonLinearConstraints;
+        }
+
+        bool OptimizationProblemInfo::hasLinearCost() const
+        {
+            return m_data->hasLinearCost;
+        }
+
+        bool OptimizationProblemInfo::hasQuadraticCost() const
+        {
+            return m_data->hasQuadraticCost;
+        }
+
+        bool OptimizationProblemInfo::hasNonLinearCost() const
+        {
+            return m_data->hasNonLinearCost;
+        }
+
+        bool OptimizationProblemInfo::hasSparseConstraintJacobian() const
+        {
+            return m_data->hasSparseConstraintJacobian;
+        }
+
+        bool OptimizationProblemInfo::hasSparseHessian() const
+        {
+            return m_data->hasSparseHessian;
+        }
+
+        bool OptimizationProblemInfo::hessianIsProvided() const
+        {
+            return m_data->hessianIsProvided;
+        }
+
     }
 }

From a2ff2d367f267bb46854997a3b1a3efb6ddaf657 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 31 May 2018 18:32:09 +0200
Subject: [PATCH 009/194] Started handling quadratic and l2-norm costs.

---
 src/optimalcontrol/CMakeLists.txt             |  10 +-
 .../include/iDynTree/L2NormCost.h             |  65 ++++++++
 .../include/iDynTree/QuadraticCost.h          | 111 ++++++-------
 .../include/iDynTree/TimeVaryingObject.h      |  50 ++++++
 src/optimalcontrol/src/L2NormCost.cpp         | 149 ++++++++++++++++++
 src/optimalcontrol/src/QuadraticCost.cpp      | 144 +++++++++++++----
 6 files changed, 442 insertions(+), 87 deletions(-)
 create mode 100644 src/optimalcontrol/include/iDynTree/L2NormCost.h
 create mode 100644 src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
 create mode 100644 src/optimalcontrol/src/L2NormCost.cpp

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index e6815e1a3a7..3d698179419 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -4,11 +4,6 @@ project(idyntree_optimalcontrol)
 
 set(libraryname idyntree-optimalcontrol)
 
-# Dependencies
-# IPOPT
-# SUNDIALS (?)
-# find_package(IPOPT REQUIRED)
-
 
 set(PUBLIC_HEADERS include/iDynTree/OptimalControl.h
                    include/iDynTree/DynamicalSystem.h
@@ -24,12 +19,14 @@ set(PUBLIC_HEADERS include/iDynTree/OptimalControl.h
                    include/iDynTree/Cost.h
                    include/iDynTree/Constraint.h
                    include/iDynTree/QuadraticCost.h
+                   include/iDynTree/L2NormCost.h
                    include/iDynTree/LinearConstraint.h
                    include/iDynTree/Controller.h
                    include/iDynTree/TimeRange.h
                    include/iDynTree/ConstraintsGroup.h
                    include/iDynTree/OptimizationProblem.h
-                   include/iDynTree/Optimizer.h)
+                   include/iDynTree/Optimizer.h
+                   include/iDynTree/TimeVaryingObject.h)
 set(INTEGRATORS_PUBLIC_HEADERS include/iDynTree/Integrators/FixedStepIntegrator.h
                                include/iDynTree/Integrators/RK4.h
                                include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -51,6 +48,7 @@ set(SOURCES src/DynamicalSystem.cpp
             src/Cost.cpp
             src/Constraint.cpp
             src/QuadraticCost.cpp
+            src/L2NormCost.cpp
             src/LinearConstraint.cpp
             src/Controller.cpp
             src/FixedStepIntegrator.cpp
diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
new file mode 100644
index 00000000000..e07d26212ac
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+
+#ifndef IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
+#define IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
+
+#include <iDynTree/QuadraticCost.h>
+#include <iDynTree/TimeVaryingObject.h>
+#include <string>
+
+namespace iDynTree {
+
+    class MatrixDynSize;
+    class VectorDynSize;
+
+    namespace optimalcontrol {
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+
+        class L2NormCost : public QuadraticCost {
+            class L2NormCostImplementation;
+            L2NormCostImplementation *m_pimpl;
+
+            using QuadraticCost::setStateCost;
+            using QuadraticCost::setControlCost;
+
+        public:
+            L2NormCost(const std::string& name);
+
+            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector); //stateSelector premultiplies the state in the L2-norm
+
+            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector, const MatrixDynSize& controlSelector); //controlSelector premultiplies the control in the L2-norm
+
+            ~L2NormCost();
+
+            bool setStateWeight(const VectorDynSize& stateWeights);
+
+            bool setStateWeight(const MatrixDynSize& stateWeights);
+
+            bool setControlWeight(const VectorDynSize& controlWeights);
+
+            bool setControlWeight(const MatrixDynSize& controlWeights);
+        };
+
+    }
+}
+
+#endif // IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index f2883401f32..9411ec58da5 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -18,72 +18,75 @@
 #define IDYNTREE_OPTIMALCONTROL_QUADRATICCOST_H
 
 #include <iDynTree/Cost.h>
-
+#include <iDynTree/TimeVaryingObject.h>
+#include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
+#include <memory>
 
 namespace iDynTree {
     namespace optimalcontrol {
+       class QuadraticCost;
+    }
+}
 
-        /**
-         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
-         * \ingroup iDynTreeExperimental
-         */
-
-        /*@
-         * Models a cost of the form
-         * \f[
-         * x^\top Q x + u^\top R u
-         * \f]
-         * where \f$x \in \mathbb{R}^n\f$ is the state and
-         * \f$u \in \mathbb{R}^n\f$ is the control
-         */
-        class QuadraticCost
-        : public Cost {
-
-            QuadraticCost(const iDynTree::MatrixDynSize& Q,
-                          const iDynTree::MatrixDynSize& R,
-                          const std::string& costName);
-
-
-            virtual bool costEvaluation(double time,
-                                        const iDynTree::VectorDynSize& state,
-                                        const iDynTree::VectorDynSize& control,
-                                        double& costValue) override;
-
-            virtual bool costFirstPartialDerivativeWRTState(double time,
-                                                            const iDynTree::VectorDynSize& state,
-                                                            const iDynTree::VectorDynSize& control,
-                                                            iDynTree::VectorDynSize& partialDerivative) override;
+/**
+ * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+ * \ingroup iDynTreeExperimental
+ */
 
-            virtual bool costFirstPartialDerivativeWRTControl(double time,
-                                                              const iDynTree::VectorDynSize& state,
-                                                              const iDynTree::VectorDynSize& control,
-                                                              iDynTree::VectorDynSize& partialDerivative) override;
+class iDynTree::optimalcontrol::QuadraticCost
+: public Cost {
 
-            virtual bool costSecondPartialDerivativeWRTState(double time,
-                                                             const iDynTree::VectorDynSize& state,
-                                                             const iDynTree::VectorDynSize& control,
-                                                             iDynTree::MatrixDynSize& partialDerivative) override;
+public:
+    QuadraticCost(const std::string& costName);
 
-            virtual bool costSecondPartialDerivativeWRTControl(double time,
-                                                               const iDynTree::VectorDynSize& state,
-                                                               const iDynTree::VectorDynSize& control,
-                                                               iDynTree::MatrixDynSize& partialDerivative) override;
+    bool setStateCost(const iDynTree::MatrixDynSize& stateHessian, const iDynTree::VectorDynSize& stateGradient);
 
+    //bool setStateCost(const iDynTree::MatrixDynSize& stateHessian,
+    //                  std::shared_ptr<ReferenceTrajectory<VectorDynSize>> timeVaryingStateGradient);
 
-            virtual bool costSecondPartialDerivativeWRTStateControl(double time,
-                                                                    const iDynTree::VectorDynSize& state,
-                                                                    const iDynTree::VectorDynSize& control,
-                                                                    iDynTree::MatrixDynSize& partialDerivative) override;
+    bool setControlCost(const iDynTree::MatrixDynSize& controlHessian, const iDynTree::VectorDynSize& controlGradient);
 
-        protected:
-            iDynTree::MatrixDynSize m_stateCostMatrix;
-            iDynTree::MatrixDynSize m_controlCostMatrix;
-            iDynTree::MatrixDynSize m_stateControlCostDerivativeMatrix;
+    //bool setControlCost(const iDynTree::MatrixDynSize& controlHessian, const iDynTree::VectorDynSize& controlGradient);
 
-        };
 
-    }
-}
+    virtual bool costEvaluation(double time,
+                                const iDynTree::VectorDynSize& state,
+                                const iDynTree::VectorDynSize& control,
+                                double& costValue) override;//0.5 xT H x + gx
+
+    virtual bool costFirstPartialDerivativeWRTState(double time,
+                                                    const iDynTree::VectorDynSize& state,
+                                                    const iDynTree::VectorDynSize& control,
+                                                    iDynTree::VectorDynSize& partialDerivative) override;
+
+    virtual bool costFirstPartialDerivativeWRTControl(double time,
+                                                      const iDynTree::VectorDynSize& state,
+                                                      const iDynTree::VectorDynSize& control,
+                                                      iDynTree::VectorDynSize& partialDerivative) override;
+
+    virtual bool costSecondPartialDerivativeWRTState(double time,
+                                                     const iDynTree::VectorDynSize& state,
+                                                     const iDynTree::VectorDynSize& control,
+                                                     iDynTree::MatrixDynSize& partialDerivative) override;
+
+    virtual bool costSecondPartialDerivativeWRTControl(double time,
+                                                       const iDynTree::VectorDynSize& state,
+                                                       const iDynTree::VectorDynSize& control,
+                                                       iDynTree::MatrixDynSize& partialDerivative) override;
+
+
+    virtual bool costSecondPartialDerivativeWRTStateControl(double time,
+                                                            const iDynTree::VectorDynSize& state,
+                                                            const iDynTree::VectorDynSize& control,
+                                                            iDynTree::MatrixDynSize& partialDerivative) override;
+
+protected:
+    iDynTree::MatrixDynSize m_stateHessian;
+    iDynTree::VectorDynSize m_stateGradient;
+    iDynTree::MatrixDynSize m_controlHessian;
+    iDynTree::VectorDynSize m_controlGradient;
+    double m_stateCostScale, m_controlCostScale;
+};
 
 #endif /* end of include guard: IDYNTREE_OPTIMALCONTROL_QUADRATICCOST_H */
diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
new file mode 100644
index 00000000000..29f75f283ab
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+
+#ifndef IDYNTREE_OPTIMALCONTROL_TIMEVARYINGOBJECT_H
+#define IDYNTREE_OPTIMALCONTROL_TIMEVARYINGOBJECT_H
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+
+namespace iDynTree {
+
+    namespace optimalcontrol {
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+        template<typename Object>
+        class TimeVaryingObject {
+        public:
+            TimeVaryingObject(){}
+
+            virtual ~TimeVaryingObject(){}
+
+            virtual bool getReference(double time, Object& objectAtTime) = 0;
+
+        };
+
+        typedef TimeVaryingObject<VectorDynSize> TimeVaryngVector;
+
+        typedef TimeVaryingObject<MatrixDynSize> TimeVaryngMatrix;
+
+    }
+}
+
+#endif // IDYNTREE_OPTIMALCONTROL_TIMEVARYINGOBJECT_H
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
new file mode 100644
index 00000000000..b0b109d29e3
--- /dev/null
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/L2NormCost.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/Utils.h>
+#include <cassert>
+
+#include <Eigen/Dense>
+#include <iDynTree/Core/EigenHelpers.h>
+
+namespace iDynTree {
+    namespace optimalcontrol {
+
+        class L2NormCost::L2NormCostImplementation {
+            public:
+            bool stateSelectorDefined = false, controlSelectorDefined = false;
+            MatrixDynSize stateSelector, controlSelector;
+            MatrixDynSize stateWeight, controlWeight;
+            MatrixDynSize stateSubMatrix, controlSubMatrix;
+            bool desiredStateDefined = false, desiredControlDefined = false;
+            VectorDynSize desiredState, desiredControl;
+            VectorDynSize stateGradient, controlGradient;
+            MatrixDynSize stateHessian, controlHessian;
+        };
+
+        L2NormCost::L2NormCost(const std::string &name)
+        : QuadraticCost(name)
+        , m_pimpl(new L2NormCostImplementation)
+        {
+            assert(m_pimpl);
+        }
+
+        L2NormCost::L2NormCost(const std::string &name, const MatrixDynSize &stateSelector)
+        : QuadraticCost(name)
+        , m_pimpl(new L2NormCostImplementation)
+        {
+            assert(m_pimpl);
+            m_pimpl->stateSelector = stateSelector;
+            m_pimpl->stateSelectorDefined = true;
+            m_pimpl->stateWeight.resize(stateSelector.rows(), stateSelector.rows());
+            toEigen(m_pimpl->stateWeight).setIdentity();
+            m_pimpl->stateSubMatrix = /*m_pimpl->stateWeight * */ stateSelector;
+            m_pimpl->desiredState.resize(stateSelector.rows());
+            m_pimpl->desiredState.zero();
+            m_pimpl->stateGradient.resize(stateSelector.cols());
+            m_pimpl->stateGradient.zero();
+            m_pimpl->stateHessian.resize(stateSelector.cols(), stateSelector.cols());
+            toEigen(m_pimpl->stateHessian) = toEigen(m_pimpl->stateSelector).transpose()*toEigen(m_pimpl->stateSubMatrix);
+        }
+
+        L2NormCost::L2NormCost(const std::string &name, const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector)
+        : QuadraticCost(name)
+        , m_pimpl(new L2NormCostImplementation)
+        {
+            assert(m_pimpl);
+            m_pimpl->stateSelector = stateSelector;
+            m_pimpl->stateSelectorDefined = true;
+            m_pimpl->stateWeight.resize(stateSelector.rows(), stateSelector.rows());
+            toEigen(m_pimpl->stateWeight).setIdentity();
+            m_pimpl->stateSubMatrix = /*m_pimpl->stateWeight * */ stateSelector;
+            m_pimpl->desiredState.resize(stateSelector.rows());
+            m_pimpl->desiredState.zero();
+            m_pimpl->stateGradient.resize(stateSelector.cols());
+            m_pimpl->stateGradient.zero();
+            m_pimpl->stateHessian.resize(stateSelector.cols(), stateSelector.cols());
+            toEigen(m_pimpl->stateHessian) = toEigen(m_pimpl->stateSelector).transpose()*toEigen(m_pimpl->stateSubMatrix);
+
+            m_pimpl->controlSelector = controlSelector;
+            m_pimpl->controlSelectorDefined = true;
+            m_pimpl->controlWeight.resize(controlSelector.rows(), controlSelector.rows());
+            toEigen(m_pimpl->controlWeight).setIdentity();
+            m_pimpl->controlSubMatrix = /*m_pimpl->controlWeight * */ controlSelector;
+            m_pimpl->desiredControl.resize(controlSelector.rows());
+            m_pimpl->desiredControl.zero();
+            m_pimpl->controlGradient.resize(controlSelector.cols());
+            m_pimpl->controlGradient.zero();
+            m_pimpl->controlHessian.resize(controlSelector.cols(), controlSelector.cols());
+            toEigen(m_pimpl->controlHessian) = toEigen(m_pimpl->controlSelector).transpose()*toEigen(m_pimpl->controlSubMatrix);
+        }
+
+        L2NormCost::~L2NormCost()
+        {
+            if (m_pimpl){
+                delete m_pimpl;
+                m_pimpl = nullptr;
+            }
+        }
+
+        bool L2NormCost::setStateWeight(const VectorDynSize &stateWeights)
+        {
+
+        }
+
+        bool L2NormCost::setStateWeight(const MatrixDynSize &stateWeights)
+        {
+            if (stateWeights.rows() != stateWeights.cols()) {
+                reportError("L2NormCost", "setStateWeight", "The stateWeights matrix is supposed to be squared.");
+                return false;
+            }
+
+            if (m_pimpl->stateSelectorDefined) {
+
+                if (stateWeights.cols() != m_pimpl->stateSelector.rows()) {
+                    reportError("L2NormCost", "setStateWeight", "The stateWeights matrix dimensions do not match those of the specified state selector.");
+                    return false;
+                }
+
+                m_pimpl->stateWeight = stateWeights;
+                toEigen(m_pimpl->stateSubMatrix) = toEigen(stateWeights) * toEigen(m_pimpl->stateSelector);
+                toEigen(m_pimpl->stateGradient) = -1.0 * toEigen(m_pimpl->desiredState).transpose() * toEigen(m_pimpl->stateSubMatrix);
+                toEigen(m_pimpl->stateHessian) = toEigen(m_pimpl->stateSelector).transpose()*toEigen(m_pimpl->stateSubMatrix);
+
+                return setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
+            } else {
+
+                m_pimpl->stateWeight = stateWeights;
+                m_pimpl->stateSubMatrix = stateWeights;
+                m_pimpl->stateHessian = stateWeights;
+
+                if (m_pimpl->desiredStateDefined) {
+                    toEigen(m_pimpl->stateGradient) = -1.0 * toEigen(m_pimpl->desiredState).transpose() * toEigen(m_pimpl->stateSubMatrix);
+                } else {
+                    m_pimpl->stateGradient.resize(stateWeights.rows());
+                }
+
+                return setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
+            }
+
+        }
+
+
+    }
+}
+
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index 30eff540eb9..e5f169ac98c 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -15,20 +15,56 @@
  */
 
 #include <iDynTree/QuadraticCost.h>
+#include <iDynTree/Core/Utils.h>
 
 #include <iDynTree/Core/EigenHelpers.h>
+#include <cmath>
 
 namespace iDynTree {
     namespace optimalcontrol {
 
-        QuadraticCost::QuadraticCost(const iDynTree::MatrixDynSize& Q,
-                                     const iDynTree::MatrixDynSize& R, const std::string &costName)
-        : Cost(costName)
-        , m_stateCostMatrix(Q)
-        , m_controlCostMatrix(R)
-        , m_stateControlCostDerivativeMatrix(Q.rows(), R.rows())
+        QuadraticCost::QuadraticCost(const std::string &costName)
+            : Cost(costName)
+            , m_stateCostScale(0.0)
+            , m_controlCostScale(0.0)
+        { }
+
+        bool QuadraticCost::setStateCost(const MatrixDynSize &stateHessian, const VectorDynSize &stateGradient)
         {
-            m_stateControlCostDerivativeMatrix.zero();
+            if (stateHessian.rows() != stateHessian.cols()) {
+                reportError("QuadraticCost", "setStateCost", "The stateHessian matrix is supposed to be square.");
+                return false;
+            }
+
+            if (stateHessian.rows() != stateGradient.size()) {
+                reportError("QuadraticCost", "setStateCost", "stateHessian and stateGradient have inconsistent dimensions.");
+                return false;
+            }
+
+            m_stateHessian = stateHessian;
+            m_stateGradient = stateGradient;
+            m_stateCostScale = 1.0;
+
+            return true;
+        }
+
+        bool QuadraticCost::setControlCost(const MatrixDynSize &controlHessian, const VectorDynSize &controlGradient)
+        {
+            if (controlHessian.rows() != controlHessian.cols()) {
+                reportError("QuadraticCost", "setControlCost", "The controlHessian matrix is supposed to be square.");
+                return false;
+            }
+
+            if (controlHessian.rows() != controlGradient.size()) {
+                reportError("QuadraticCost", "setControlCost", "controlHessian and controlGradient have inconsistent dimensions.");
+                return false;
+            }
+
+            m_controlHessian = controlHessian;
+            m_controlGradient = controlGradient;
+            m_controlCostScale = 1.0;
+
+            return true;
         }
 
         bool QuadraticCost::costEvaluation(double /*time*/,
@@ -36,58 +72,112 @@ namespace iDynTree {
                                            const iDynTree::VectorDynSize& control,
                                            double& costValue)
         {
-            costValue = 0.5 * (iDynTree::toEigen(state).transpose() * iDynTree::toEigen(m_stateCostMatrix) * iDynTree::toEigen(state)
-                          + iDynTree::toEigen(control).transpose() * iDynTree::toEigen(m_controlCostMatrix) * iDynTree::toEigen(control))(0);
+            double stateCost = 0, controlCost = 0;
+
+            if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
+                if (m_stateHessian.rows() != state.size()) {
+                    reportError("QuadraticCost", "costEvaluation", "The specified state hessian matrix dimensions do not match the state dimension.");
+                    return false;
+                }
+                stateCost = m_stateCostScale * 0.5 * (toEigen(state).transpose() * toEigen(m_stateHessian) * toEigen(state))(0) + toEigen(m_stateGradient).transpose()*toEigen(state);
+            }
+
+            if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
+                if (m_controlHessian.rows() != control.size()) {
+                    reportError("QuadraticCost", "costEvaluation", "The specified control hessian matrix dimensions do not match the control dimension.");
+                    return false;
+                }
+                controlCost = m_controlCostScale * 0.5 * (toEigen(control).transpose() * toEigen(m_controlHessian) * toEigen(control))(0) + toEigen(m_controlGradient).transpose()*toEigen(control);
+            }
+
+            costValue = stateCost + controlCost;
             return true;
         }
 
-        bool QuadraticCost::costFirstPartialDerivativeWRTState(double time,
+        bool QuadraticCost::costFirstPartialDerivativeWRTState(double /*time*/,
                                                                const iDynTree::VectorDynSize& state,
-                                                               const iDynTree::VectorDynSize& control,
+                                                               const iDynTree::VectorDynSize& /*control*/,
                                                                iDynTree::VectorDynSize& partialDerivative)
         {
-            // TODO: to be checked with the formalism needed: A x or x^T A ?
-            iDynTree::toEigen(partialDerivative) = iDynTree::toEigen(m_stateCostMatrix) * iDynTree::toEigen(state);
+            partialDerivative.resize(state.size());
+
+            if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
+                if (m_stateHessian.rows() != state.size()) {
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", "The specified state hessian matrix dimensions do not match the state dimension.");
+                    return false;
+                }
+                toEigen(partialDerivative) = m_stateCostScale * toEigen(m_stateHessian) * toEigen(state) + toEigen(m_stateGradient);
+            } else {
+                partialDerivative.zero();
+            }
+
             return true;
         }
 
-        bool QuadraticCost::costFirstPartialDerivativeWRTControl(double time,
-                                                          const iDynTree::VectorDynSize& state,
+        bool QuadraticCost::costFirstPartialDerivativeWRTControl(double /*time*/,
+                                                          const iDynTree::VectorDynSize& /*state*/,
                                                           const iDynTree::VectorDynSize& control,
                                                           iDynTree::VectorDynSize& partialDerivative)
         {
-            // TODO: to be checked with the formalism needed: A x or x^T A ?
-            iDynTree::toEigen(partialDerivative) = iDynTree::toEigen(m_controlCostMatrix) * iDynTree::toEigen(control);
+            partialDerivative.resize(control.size());
+
+            if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
+                if (m_controlHessian.rows() != control.size()) {
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", "The specified control hessian matrix dimensions do not match the control dimension.");
+                    return false;
+                }
+                toEigen(partialDerivative) = m_controlCostScale * toEigen(m_controlHessian) * toEigen(control) + toEigen(m_controlGradient);
+            } else {
+                partialDerivative.zero();
+            }
             return true;
         }
 
-        bool QuadraticCost::costSecondPartialDerivativeWRTState(double time,
+        bool QuadraticCost::costSecondPartialDerivativeWRTState(double /*time*/,
                                                          const iDynTree::VectorDynSize& state,
-                                                         const iDynTree::VectorDynSize& control,
+                                                         const iDynTree::VectorDynSize& /*control*/,
                                                          iDynTree::MatrixDynSize& partialDerivative)
         {
-            partialDerivative = m_stateCostMatrix;
+            partialDerivative.resize(state.size(), state.size());
+            if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
+                if (m_stateHessian.rows() != state.size()) {
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", "The specified state hessian matrix dimensions do not match the state dimension.");
+                    return false;
+                }
+                toEigen(partialDerivative) = m_stateCostScale * toEigen(m_stateHessian);
+            } else {
+                partialDerivative.zero();
+            }
             return true;
         }
 
-        bool QuadraticCost::costSecondPartialDerivativeWRTControl(double time,
-                                                           const iDynTree::VectorDynSize& state,
+        bool QuadraticCost::costSecondPartialDerivativeWRTControl(double /*time*/,
+                                                           const iDynTree::VectorDynSize& /*state*/,
                                                            const iDynTree::VectorDynSize& control,
                                                            iDynTree::MatrixDynSize& partialDerivative)
         {
-            partialDerivative = m_controlCostMatrix;
+            partialDerivative.resize(control.size(), control.size());
+
+            if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
+                if (m_controlHessian.rows() != control.size()) {
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", "The specified control hessian matrix dimensions do not match the control dimension.");
+                    return false;
+                }
+                toEigen(partialDerivative) = m_controlCostScale * toEigen(m_controlHessian);
+            } else {
+                partialDerivative.zero();
+            }
             return true;
         }
 
 
-        bool QuadraticCost::costSecondPartialDerivativeWRTStateControl(double time,
+        bool QuadraticCost::costSecondPartialDerivativeWRTStateControl(double /*time*/,
                                                                 const iDynTree::VectorDynSize& state,
                                                                 const iDynTree::VectorDynSize& control,
                                                                 iDynTree::MatrixDynSize& partialDerivative)
         {
-            // return empty matrix
-            // or simply zero the matrix??
-            partialDerivative = m_stateControlCostDerivativeMatrix;
+            partialDerivative.resize(state.size(), control.size());
+            partialDerivative.zero();
             return true;
         }
 

From 26eabeda79025f56488515a4699700e4771349a1 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 1 Jun 2018 16:54:04 +0200
Subject: [PATCH 010/194] Implemented QuadraticCost and L2NormCost.

---
 src/optimalcontrol/CMakeLists.txt             |   3 +-
 .../include/iDynTree/L2NormCost.h             |  21 +-
 .../include/iDynTree/QuadraticCost.h          |  95 +++---
 .../include/iDynTree/TimeVaryingObject.h      |  36 ++-
 src/optimalcontrol/src/L2NormCost.cpp         | 302 +++++++++++++-----
 src/optimalcontrol/src/QuadraticCost.cpp      | 271 +++++++++++++---
 src/optimalcontrol/src/TimeVaryingObject.cpp  |  58 ++++
 src/optimalcontrol/tests/IntegratorsTest.cpp  |  42 +++
 8 files changed, 659 insertions(+), 169 deletions(-)
 create mode 100644 src/optimalcontrol/src/TimeVaryingObject.cpp

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 3d698179419..1eec85c7022 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -58,7 +58,8 @@ set(SOURCES src/DynamicalSystem.cpp
             src/ConstraintsGroup.cpp
             src/OptimizationProblem.cpp
             src/Optimizer.cpp
-            src/ForwardEuler.cpp)
+            src/ForwardEuler.cpp
+            src/TimeVaryingObject.cpp)
 
 if (IDYNTREE_USES_IPOPT)
     list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/IpoptInterface.h)
diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index e07d26212ac..f40f1b2e742 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -20,6 +20,7 @@
 
 #include <iDynTree/QuadraticCost.h>
 #include <iDynTree/TimeVaryingObject.h>
+#include <memory>
 #include <string>
 
 namespace iDynTree {
@@ -38,25 +39,31 @@ namespace iDynTree {
             class L2NormCostImplementation;
             L2NormCostImplementation *m_pimpl;
 
-            using QuadraticCost::setStateCost;
+            using QuadraticCost::setStateCost;  //avoid the user to access these methods to avoid confusion
             using QuadraticCost::setControlCost;
 
         public:
-            L2NormCost(const std::string& name);
 
-            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector); //stateSelector premultiplies the state in the L2-norm
+            L2NormCost(const std::string& name, unsigned int stateDimension, unsigned int controlDimension); //assume identity as selector. Set the dimension to 0 to avoid weighting either the state or the cost
 
-            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector, const MatrixDynSize& controlSelector); //controlSelector premultiplies the control in the L2-norm
+            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector, const MatrixDynSize& controlSelector); //The selector premultiplies the state and the control in the L2-norm. Set some dimension to 0 to avoid weighting either the state or the cost
 
-            ~L2NormCost();
+            L2NormCost(const L2NormCost& other) = delete;
 
-            bool setStateWeight(const VectorDynSize& stateWeights);
+            ~L2NormCost();
 
             bool setStateWeight(const MatrixDynSize& stateWeights);
 
-            bool setControlWeight(const VectorDynSize& controlWeights);
+            bool setStateDesiredPoint(const VectorDynSize& desiredPoint);
+
+            bool setStateDesiredTrajectory(std::shared_ptr<TimeVaryingVector> stateDesiredTrajectory);
 
             bool setControlWeight(const MatrixDynSize& controlWeights);
+
+            bool setControlDesiredPoint(const VectorDynSize& desiredPoint);
+
+            bool setControlDesiredTrajectory(std::shared_ptr<TimeVaryingVector> controlDesiredTrajectory);
+
         };
 
     }
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index 9411ec58da5..ba353f1dabf 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -25,68 +25,69 @@
 
 namespace iDynTree {
     namespace optimalcontrol {
-       class QuadraticCost;
-    }
-}
-
-/**
- * @warning This class is still in active development, and so API interface can change between iDynTree versions.
- * \ingroup iDynTreeExperimental
- */
-
-class iDynTree::optimalcontrol::QuadraticCost
-: public Cost {
-
-public:
-    QuadraticCost(const std::string& costName);
 
-    bool setStateCost(const iDynTree::MatrixDynSize& stateHessian, const iDynTree::VectorDynSize& stateGradient);
+       /**
+        * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+        * \ingroup iDynTreeExperimental
+        */
 
-    //bool setStateCost(const iDynTree::MatrixDynSize& stateHessian,
-    //                  std::shared_ptr<ReferenceTrajectory<VectorDynSize>> timeVaryingStateGradient);
+       class QuadraticCost
+       : public Cost {
 
-    bool setControlCost(const iDynTree::MatrixDynSize& controlHessian, const iDynTree::VectorDynSize& controlGradient);
+       public:
+           QuadraticCost(const std::string& costName);
 
-    //bool setControlCost(const iDynTree::MatrixDynSize& controlHessian, const iDynTree::VectorDynSize& controlGradient);
+           virtual ~QuadraticCost() override;
 
+           bool setStateCost(const iDynTree::MatrixDynSize& stateHessian, const iDynTree::VectorDynSize& stateGradient);
 
-    virtual bool costEvaluation(double time,
-                                const iDynTree::VectorDynSize& state,
-                                const iDynTree::VectorDynSize& control,
-                                double& costValue) override;//0.5 xT H x + gx
+           bool setStateCost(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> timeVaryingStateHessian,
+                             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> timeVaryingStateGradient);
 
-    virtual bool costFirstPartialDerivativeWRTState(double time,
-                                                    const iDynTree::VectorDynSize& state,
-                                                    const iDynTree::VectorDynSize& control,
-                                                    iDynTree::VectorDynSize& partialDerivative) override;
+           bool setControlCost(const iDynTree::MatrixDynSize& controlHessian, const iDynTree::VectorDynSize& controlGradient);
 
-    virtual bool costFirstPartialDerivativeWRTControl(double time,
-                                                      const iDynTree::VectorDynSize& state,
-                                                      const iDynTree::VectorDynSize& control,
-                                                      iDynTree::VectorDynSize& partialDerivative) override;
+           bool setControlCost(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> timeVaryingControlHessian,
+                               std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> timeVaryingControlGradient);
 
-    virtual bool costSecondPartialDerivativeWRTState(double time,
-                                                     const iDynTree::VectorDynSize& state,
-                                                     const iDynTree::VectorDynSize& control,
-                                                     iDynTree::MatrixDynSize& partialDerivative) override;
+           virtual bool costEvaluation(double time,
+                                       const iDynTree::VectorDynSize& state,
+                                       const iDynTree::VectorDynSize& control,
+                                       double& costValue) override;//0.5 xT H x + gx
 
-    virtual bool costSecondPartialDerivativeWRTControl(double time,
-                                                       const iDynTree::VectorDynSize& state,
-                                                       const iDynTree::VectorDynSize& control,
-                                                       iDynTree::MatrixDynSize& partialDerivative) override;
+           virtual bool costFirstPartialDerivativeWRTState(double time,
+                                                           const iDynTree::VectorDynSize& state,
+                                                           const iDynTree::VectorDynSize& control,
+                                                           iDynTree::VectorDynSize& partialDerivative) override;
 
+           virtual bool costFirstPartialDerivativeWRTControl(double time,
+                                                             const iDynTree::VectorDynSize& state,
+                                                             const iDynTree::VectorDynSize& control,
+                                                             iDynTree::VectorDynSize& partialDerivative) override;
 
-    virtual bool costSecondPartialDerivativeWRTStateControl(double time,
+           virtual bool costSecondPartialDerivativeWRTState(double time,
                                                             const iDynTree::VectorDynSize& state,
                                                             const iDynTree::VectorDynSize& control,
                                                             iDynTree::MatrixDynSize& partialDerivative) override;
 
-protected:
-    iDynTree::MatrixDynSize m_stateHessian;
-    iDynTree::VectorDynSize m_stateGradient;
-    iDynTree::MatrixDynSize m_controlHessian;
-    iDynTree::VectorDynSize m_controlGradient;
-    double m_stateCostScale, m_controlCostScale;
-};
+           virtual bool costSecondPartialDerivativeWRTControl(double time,
+                                                              const iDynTree::VectorDynSize& state,
+                                                              const iDynTree::VectorDynSize& control,
+                                                              iDynTree::MatrixDynSize& partialDerivative) override;
+
+
+           virtual bool costSecondPartialDerivativeWRTStateControl(double time,
+                                                                   const iDynTree::VectorDynSize& state,
+                                                                   const iDynTree::VectorDynSize& control,
+                                                                   iDynTree::MatrixDynSize& partialDerivative) override;
+
+       protected:
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingStateHessian;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingStateGradient;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
+           double m_stateCostScale, m_controlCostScale;
+       };
+    }
+}
 
 #endif /* end of include guard: IDYNTREE_OPTIMALCONTROL_QUADRATICCOST_H */
diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
index 29f75f283ab..222c7e9aed2 100644
--- a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
+++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
@@ -34,15 +34,43 @@ namespace iDynTree {
         public:
             TimeVaryingObject(){}
 
-            virtual ~TimeVaryingObject(){}
+            virtual ~TimeVaryingObject() {}
 
-            virtual bool getReference(double time, Object& objectAtTime) = 0;
+            virtual const Object& getObject(double time, bool &isValid) = 0;
 
         };
 
-        typedef TimeVaryingObject<VectorDynSize> TimeVaryngVector;
+        typedef TimeVaryingObject<VectorDynSize> TimeVaryingVector;
 
-        typedef TimeVaryingObject<MatrixDynSize> TimeVaryngMatrix;
+        typedef TimeVaryingObject<MatrixDynSize> TimeVaryingMatrix;
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+        class TimeInvariantVector : public TimeVaryingVector {
+            VectorDynSize m_timeInvariantVector;
+        public:
+            TimeInvariantVector();
+
+            TimeInvariantVector(const VectorDynSize& timeInvariantVector);
+
+            VectorDynSize& get();
+
+            virtual const VectorDynSize& getObject(double time, bool &isValid) override;
+        };
+
+        class TimeInvariantMatrix : public TimeVaryingMatrix {
+            MatrixDynSize m_timeInvariantMatrix;
+        public:
+            TimeInvariantMatrix();
+
+            TimeInvariantMatrix(const MatrixDynSize& timeInvariantMatrix);
+
+            MatrixDynSize& get();
+
+            virtual const MatrixDynSize& getObject(double time, bool &isValid) override;
+        };
 
     }
 }
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index b0b109d29e3..7cbe7f056f7 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -18,6 +18,7 @@
 #include <iDynTree/Core/MatrixDynSize.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/TestUtils.h>
 #include <cassert>
 
 #include <Eigen/Dense>
@@ -26,71 +27,165 @@
 namespace iDynTree {
     namespace optimalcontrol {
 
+    class TimeVaryingGradient : public TimeVaryingVector {
+        MatrixDynSize m_selectorMatrix, m_weightMatrix;
+        MatrixDynSize m_subMatrix; //weightMatrix times selector
+        std::shared_ptr<TimeVaryingVector> m_desiredTrajectory;
+        VectorDynSize m_outputVector;
+    public:
+        TimeVaryingGradient(const MatrixDynSize& selectorMatrix)
+        : m_selectorMatrix(selectorMatrix)
+        , m_desiredTrajectory(nullptr)
+        {
+            m_weightMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.rows());
+            toEigen(m_weightMatrix).setIdentity();
+            m_subMatrix = /*m_pimpl->stateWeight * */ m_selectorMatrix;
+            m_outputVector.resize(m_selectorMatrix.cols());
+            m_outputVector.zero();
+        }
+
+        ~TimeVaryingGradient() override;
+
+        bool setDesiredTrajectory(std::shared_ptr<TimeVaryingVector> desiredTrajectory) {
+            if (!desiredTrajectory) {
+                reportError("TimeVaryingGradient", "desiredTrajectory", "Empty desired trajectory pointer.");
+                return false;
+            }
+            m_desiredTrajectory = desiredTrajectory;
+            return true;
+        }
+
+        bool setWeightMatrix(const MatrixDynSize &weights)
+        {
+            if (weights.rows() != weights.cols()) {
+                reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix is supposed to be squared.");
+                return false;
+            }
+
+            if (weights.cols() != m_selectorMatrix.rows()) {
+                reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix dimensions do not match those of the specified selector.");
+                return false;
+            }
+
+            m_weightMatrix = weights;
+            toEigen(m_subMatrix) = toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+
+            return true;
+        }
+
+        virtual const VectorDynSize& getObject(double time, bool &isValid) override {
+            if (!m_desiredTrajectory) {
+                isValid = true;
+                return m_outputVector; //should be zero from the initialization
+            }
+            bool ok = false;
+            const VectorDynSize &desiredPoint = m_desiredTrajectory->getObject(time, ok);
+
+            if (!ok) {
+                isValid = false;
+                m_outputVector.zero();
+                return m_outputVector;
+            }
+
+            if (desiredPoint.size() != m_subMatrix.rows()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The specified desired point at time: " << time << " has size not matching the specified selector.";
+                reportError("TimeVaryingGradient", "getObject", errorMsg.str().c_str());
+                isValid = false;
+                m_outputVector.zero();
+                return m_outputVector;
+            }
+
+            toEigen(m_outputVector) = -1.0 * toEigen(desiredPoint).transpose() * toEigen(m_subMatrix);
+
+            isValid = true;
+            return m_outputVector;
+        }
+
+        const MatrixDynSize& selector() {
+            return m_selectorMatrix;
+        }
+
+        const MatrixDynSize& subMatrix() {
+            return m_subMatrix;
+        }
+    };
+    TimeVaryingGradient::~TimeVaryingGradient() {};
+
+
         class L2NormCost::L2NormCostImplementation {
             public:
-            bool stateSelectorDefined = false, controlSelectorDefined = false;
-            MatrixDynSize stateSelector, controlSelector;
-            MatrixDynSize stateWeight, controlWeight;
-            MatrixDynSize stateSubMatrix, controlSubMatrix;
-            bool desiredStateDefined = false, desiredControlDefined = false;
-            VectorDynSize desiredState, desiredControl;
-            VectorDynSize stateGradient, controlGradient;
-            MatrixDynSize stateHessian, controlHessian;
+            std::shared_ptr<TimeVaryingGradient> stateGradient, controlGradient;
+            std::shared_ptr<TimeInvariantMatrix> stateHessian, controlHessian;
+
+            void initialize(const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector) {
+                if ((stateSelector.rows() != 0) && (stateSelector.cols() != 0)) {
+                    stateGradient = std::make_shared<TimeVaryingGradient>(stateSelector);
+                    stateHessian = std::make_shared<TimeInvariantMatrix>();
+                    stateHessian->get().resize(stateSelector.cols(), stateSelector.cols());
+                    toEigen(stateHessian->get()) = toEigen(stateGradient->selector()).transpose() * toEigen(stateGradient->subMatrix());
+                } else {
+                    stateGradient = nullptr;
+                }
+
+                if ((controlSelector.rows() != 0) && (controlSelector.cols() != 0)) {
+                    controlGradient = std::make_shared<TimeVaryingGradient>(controlSelector);
+                    controlHessian = std::make_shared<TimeInvariantMatrix>();
+                    controlHessian->get().resize(controlSelector.cols(), controlSelector.cols());
+                    toEigen(controlHessian->get()) = toEigen(controlGradient->selector()).transpose() * toEigen(controlGradient->subMatrix());
+                } else {
+                    controlGradient = nullptr;
+                }
+            }
+
+            void initialize(unsigned int stateDimension, unsigned int controlDimension) {
+                MatrixDynSize stateSelector(stateDimension, stateDimension), controlSelector(controlDimension, controlDimension);
+                toEigen(stateSelector).setIdentity();
+                toEigen(controlSelector).setIdentity();
+                initialize(stateSelector, controlSelector);
+            }
+
         };
 
-        L2NormCost::L2NormCost(const std::string &name)
-        : QuadraticCost(name)
-        , m_pimpl(new L2NormCostImplementation)
-        {
-            assert(m_pimpl);
-        }
 
-        L2NormCost::L2NormCost(const std::string &name, const MatrixDynSize &stateSelector)
+        L2NormCost::L2NormCost(const std::string &name, unsigned int stateDimension, unsigned int controlDimension)
         : QuadraticCost(name)
         , m_pimpl(new L2NormCostImplementation)
         {
             assert(m_pimpl);
-            m_pimpl->stateSelector = stateSelector;
-            m_pimpl->stateSelectorDefined = true;
-            m_pimpl->stateWeight.resize(stateSelector.rows(), stateSelector.rows());
-            toEigen(m_pimpl->stateWeight).setIdentity();
-            m_pimpl->stateSubMatrix = /*m_pimpl->stateWeight * */ stateSelector;
-            m_pimpl->desiredState.resize(stateSelector.rows());
-            m_pimpl->desiredState.zero();
-            m_pimpl->stateGradient.resize(stateSelector.cols());
-            m_pimpl->stateGradient.zero();
-            m_pimpl->stateHessian.resize(stateSelector.cols(), stateSelector.cols());
-            toEigen(m_pimpl->stateHessian) = toEigen(m_pimpl->stateSelector).transpose()*toEigen(m_pimpl->stateSubMatrix);
+
+            m_pimpl->initialize(stateDimension, controlDimension);
+
+            if (m_pimpl->stateGradient) {
+                bool ok;
+                ok = setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
+                ASSERT_IS_TRUE(ok);
+            }
+
+            if (m_pimpl->controlGradient) {
+                bool ok;
+                ok = setControlCost(m_pimpl->controlHessian, m_pimpl->controlGradient);
+                ASSERT_IS_TRUE(ok);
+            }
         }
 
         L2NormCost::L2NormCost(const std::string &name, const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector)
         : QuadraticCost(name)
         , m_pimpl(new L2NormCostImplementation)
         {
-            assert(m_pimpl);
-            m_pimpl->stateSelector = stateSelector;
-            m_pimpl->stateSelectorDefined = true;
-            m_pimpl->stateWeight.resize(stateSelector.rows(), stateSelector.rows());
-            toEigen(m_pimpl->stateWeight).setIdentity();
-            m_pimpl->stateSubMatrix = /*m_pimpl->stateWeight * */ stateSelector;
-            m_pimpl->desiredState.resize(stateSelector.rows());
-            m_pimpl->desiredState.zero();
-            m_pimpl->stateGradient.resize(stateSelector.cols());
-            m_pimpl->stateGradient.zero();
-            m_pimpl->stateHessian.resize(stateSelector.cols(), stateSelector.cols());
-            toEigen(m_pimpl->stateHessian) = toEigen(m_pimpl->stateSelector).transpose()*toEigen(m_pimpl->stateSubMatrix);
-
-            m_pimpl->controlSelector = controlSelector;
-            m_pimpl->controlSelectorDefined = true;
-            m_pimpl->controlWeight.resize(controlSelector.rows(), controlSelector.rows());
-            toEigen(m_pimpl->controlWeight).setIdentity();
-            m_pimpl->controlSubMatrix = /*m_pimpl->controlWeight * */ controlSelector;
-            m_pimpl->desiredControl.resize(controlSelector.rows());
-            m_pimpl->desiredControl.zero();
-            m_pimpl->controlGradient.resize(controlSelector.cols());
-            m_pimpl->controlGradient.zero();
-            m_pimpl->controlHessian.resize(controlSelector.cols(), controlSelector.cols());
-            toEigen(m_pimpl->controlHessian) = toEigen(m_pimpl->controlSelector).transpose()*toEigen(m_pimpl->controlSubMatrix);
+            m_pimpl->initialize(stateSelector, controlSelector);
+
+            if (m_pimpl->stateGradient) {
+                bool ok;
+                ok = setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
+                ASSERT_IS_TRUE(ok);
+            }
+
+            if (m_pimpl->controlGradient) {
+                bool ok;
+                ok = setControlCost(m_pimpl->controlHessian, m_pimpl->controlGradient);
+                ASSERT_IS_TRUE(ok);
+            }
         }
 
         L2NormCost::~L2NormCost()
@@ -101,48 +196,107 @@ namespace iDynTree {
             }
         }
 
-        bool L2NormCost::setStateWeight(const VectorDynSize &stateWeights)
+        bool L2NormCost::setStateWeight(const MatrixDynSize &stateWeights)
         {
+            if (stateWeights.rows() != stateWeights.cols()) {
+                reportError("L2NormCost", "setStateWeight", "The stateWeights matrix is supposed to be squared.");
+                return false;
+            }
+
+            if (!(m_pimpl->stateGradient)) {
+                reportError("L2NormCost", "setStateWeight", "The state cost portion has been deactivated, given the provided selectors.");
+                return false;
+            }
+
+            if (!(m_pimpl->stateGradient->setWeightMatrix(stateWeights))) {
+                reportError("L2NormCost", "setStateWeight", "Error when specifying the state weights.");
+                return false;
+            }
 
+            return true;
         }
 
-        bool L2NormCost::setStateWeight(const MatrixDynSize &stateWeights)
+        bool L2NormCost::setStateDesiredPoint(const VectorDynSize &desiredPoint)
         {
-            if (stateWeights.rows() != stateWeights.cols()) {
-                reportError("L2NormCost", "setStateWeight", "The stateWeights matrix is supposed to be squared.");
+            if (!(m_pimpl->stateGradient)) {
+                reportError("L2NormCost", "setStateDesiredPoint", "The state cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
 
-            if (m_pimpl->stateSelectorDefined) {
+            if (desiredPoint.size() != (m_pimpl->stateGradient->subMatrix().rows())) {
+                reportError("L2NormCost", "setStateDesiredPoint", "The desiredPoint size do not match the dimension of the specified selector.");
+                return false;
+            }
 
-                if (stateWeights.cols() != m_pimpl->stateSelector.rows()) {
-                    reportError("L2NormCost", "setStateWeight", "The stateWeights matrix dimensions do not match those of the specified state selector.");
-                    return false;
-                }
+            std::shared_ptr<TimeInvariantVector> newTrajectory = std::make_shared<TimeInvariantVector>(desiredPoint);
+            return m_pimpl->stateGradient->setDesiredTrajectory(newTrajectory);
+        }
 
-                m_pimpl->stateWeight = stateWeights;
-                toEigen(m_pimpl->stateSubMatrix) = toEigen(stateWeights) * toEigen(m_pimpl->stateSelector);
-                toEigen(m_pimpl->stateGradient) = -1.0 * toEigen(m_pimpl->desiredState).transpose() * toEigen(m_pimpl->stateSubMatrix);
-                toEigen(m_pimpl->stateHessian) = toEigen(m_pimpl->stateSelector).transpose()*toEigen(m_pimpl->stateSubMatrix);
+        bool L2NormCost::setStateDesiredTrajectory(std::shared_ptr<TimeVaryingVector> stateDesiredTrajectory)
+        {
+            if (!(m_pimpl->stateGradient)) {
+                reportError("L2NormCost", "setStateDesiredTrajectory", "The state cost portion has been deactivated, given the provided selectors.");
+                return false;
+            }
 
-                return setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
-            } else {
+            if (!stateDesiredTrajectory) {
+                reportError("L2NormCost", "setStateDesiredTrajectory", "Empty desired trajectory pointer.");
+                return false;
+            }
 
-                m_pimpl->stateWeight = stateWeights;
-                m_pimpl->stateSubMatrix = stateWeights;
-                m_pimpl->stateHessian = stateWeights;
+            return m_pimpl->stateGradient->setDesiredTrajectory(stateDesiredTrajectory);
+        }
 
-                if (m_pimpl->desiredStateDefined) {
-                    toEigen(m_pimpl->stateGradient) = -1.0 * toEigen(m_pimpl->desiredState).transpose() * toEigen(m_pimpl->stateSubMatrix);
-                } else {
-                    m_pimpl->stateGradient.resize(stateWeights.rows());
-                }
+        bool L2NormCost::setControlWeight(const MatrixDynSize &controlWeights)
+        {
+            if (controlWeights.rows() != controlWeights.cols()) {
+                reportError("L2NormCost", "setControlWeight", "The controlWeights matrix is supposed to be squared.");
+                return false;
+            }
+
+            if (!(m_pimpl->controlGradient)) {
+                reportError("L2NormCost", "setControlWeight", "The control cost portion has been deactivated, given the provided selectors.");
+                return false;
+            }
+
+            if (!(m_pimpl->controlGradient->setWeightMatrix(controlWeights))) {
+                reportError("L2NormCost", "setControlWeight", "Error when specifying the control weights.");
+                return false;
+            }
+
+            return true;
+        }
 
-                return setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
+        bool L2NormCost::setControlDesiredPoint(const VectorDynSize &desiredPoint)
+        {
+            if (!(m_pimpl->controlGradient)) {
+                reportError("L2NormCost", "setControlDesiredPoint", "The control cost portion has been deactivated, given the provided selectors.");
+                return false;
             }
 
+            if (desiredPoint.size() != (m_pimpl->controlGradient->subMatrix().rows())) {
+                reportError("L2NormCost", "setControlDesiredPoint", "The desiredPoint size do not match the dimension of the specified selector.");
+                return false;
+            }
+
+            std::shared_ptr<TimeInvariantVector> newTrajectory = std::make_shared<TimeInvariantVector>(desiredPoint);
+            return m_pimpl->controlGradient->setDesiredTrajectory(newTrajectory);
         }
 
+        bool L2NormCost::setControlDesiredTrajectory(std::shared_ptr<TimeVaryingVector> controlDesiredTrajectory)
+        {
+            if (!(m_pimpl->controlGradient)) {
+                reportError("L2NormCost", "setControlDesiredTrajectory", "The control cost portion has been deactivated, given the provided selectors.");
+                return false;
+            }
+
+            if (!controlDesiredTrajectory) {
+                reportError("L2NormCost", "setControlDesiredTrajectory", "Empty desired trajectory pointer.");
+                return false;
+            }
+
+            return m_pimpl->controlGradient->setDesiredTrajectory(controlDesiredTrajectory);
+        }
 
     }
 }
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index e5f169ac98c..882d0bafc1b 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -29,6 +29,9 @@ namespace iDynTree {
             , m_controlCostScale(0.0)
         { }
 
+        QuadraticCost::~QuadraticCost()
+        { }
+
         bool QuadraticCost::setStateCost(const MatrixDynSize &stateHessian, const VectorDynSize &stateGradient)
         {
             if (stateHessian.rows() != stateHessian.cols()) {
@@ -41,13 +44,31 @@ namespace iDynTree {
                 return false;
             }
 
-            m_stateHessian = stateHessian;
-            m_stateGradient = stateGradient;
+            m_timeVaryingStateHessian.reset(new TimeInvariantMatrix(stateHessian));
+            m_timeVaryingStateGradient.reset(new TimeInvariantVector(stateGradient));
             m_stateCostScale = 1.0;
 
             return true;
         }
 
+        bool QuadraticCost::setStateCost(std::shared_ptr<TimeVaryingMatrix> timeVaryingStateHessian, std::shared_ptr<TimeVaryingVector> timeVaryingStateGradient)
+        {
+            if (!timeVaryingStateHessian) {
+                reportError("QuadraticCost", "setStateCost", "Empty hessian pointer.");
+                return false;
+            }
+
+            if (!timeVaryingStateGradient) {
+                reportError("QuadraticCost", "setStateCost", "Empty gradient pointer.");
+                return false;
+            }
+
+            m_timeVaryingStateHessian = timeVaryingStateHessian;
+            m_timeVaryingStateGradient = timeVaryingStateGradient;
+
+            return true;
+        }
+
         bool QuadraticCost::setControlCost(const MatrixDynSize &controlHessian, const VectorDynSize &controlGradient)
         {
             if (controlHessian.rows() != controlHessian.cols()) {
@@ -60,14 +81,32 @@ namespace iDynTree {
                 return false;
             }
 
-            m_controlHessian = controlHessian;
-            m_controlGradient = controlGradient;
+            m_timeVaryingControlHessian.reset(new TimeInvariantMatrix(controlHessian));
+            m_timeVaryingControlGradient.reset(new TimeInvariantVector(controlGradient));
             m_controlCostScale = 1.0;
 
             return true;
         }
 
-        bool QuadraticCost::costEvaluation(double /*time*/,
+        bool QuadraticCost::setControlCost(std::shared_ptr<TimeVaryingMatrix> timeVaryingControlHessian, std::shared_ptr<TimeVaryingVector> timeVaryingControlGradient)
+        {
+            if (!timeVaryingControlHessian) {
+                reportError("QuadraticCost", "setControlCost", "Empty hessian pointer.");
+                return false;
+            }
+
+            if (!timeVaryingControlGradient) {
+                reportError("QuadraticCost", "setControlCost", "Empty gradient pointer.");
+                return false;
+            }
+
+            m_timeVaryingControlHessian = timeVaryingControlHessian;
+            m_timeVaryingControlGradient = timeVaryingControlGradient;
+
+            return true;
+        }
+
+        bool QuadraticCost::costEvaluation(double time,
                                            const iDynTree::VectorDynSize& state,
                                            const iDynTree::VectorDynSize& control,
                                            double& costValue)
@@ -75,26 +114,86 @@ namespace iDynTree {
             double stateCost = 0, controlCost = 0;
 
             if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
-                if (m_stateHessian.rows() != state.size()) {
-                    reportError("QuadraticCost", "costEvaluation", "The specified state hessian matrix dimensions do not match the state dimension.");
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingStateGradient->getObject(time, isValid);
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
                     return false;
                 }
-                stateCost = m_stateCostScale * 0.5 * (toEigen(state).transpose() * toEigen(m_stateHessian) * toEigen(state))(0) + toEigen(m_stateGradient).transpose()*toEigen(state);
+
+                stateCost = m_stateCostScale * 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0) + toEigen(gradient).transpose()*toEigen(state);
             }
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
-                if (m_controlHessian.rows() != control.size()) {
-                    reportError("QuadraticCost", "costEvaluation", "The specified control hessian matrix dimensions do not match the control dimension.");
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingControlGradient->getObject(time, isValid);
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
                     return false;
                 }
-                controlCost = m_controlCostScale * 0.5 * (toEigen(control).transpose() * toEigen(m_controlHessian) * toEigen(control))(0) + toEigen(m_controlGradient).transpose()*toEigen(control);
+
+                controlCost = m_controlCostScale * 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0) + toEigen(gradient).transpose()*toEigen(control);
             }
 
             costValue = stateCost + controlCost;
             return true;
         }
 
-        bool QuadraticCost::costFirstPartialDerivativeWRTState(double /*time*/,
+        bool QuadraticCost::costFirstPartialDerivativeWRTState(double time,
                                                                const iDynTree::VectorDynSize& state,
                                                                const iDynTree::VectorDynSize& /*control*/,
                                                                iDynTree::VectorDynSize& partialDerivative)
@@ -102,11 +201,41 @@ namespace iDynTree {
             partialDerivative.resize(state.size());
 
             if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
-                if (m_stateHessian.rows() != state.size()) {
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", "The specified state hessian matrix dimensions do not match the state dimension.");
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
                     return false;
                 }
-                toEigen(partialDerivative) = m_stateCostScale * toEigen(m_stateHessian) * toEigen(state) + toEigen(m_stateGradient);
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingStateGradient->getObject(time, isValid);
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                toEigen(partialDerivative) = m_stateCostScale * toEigen(hessian) * toEigen(state) + toEigen(gradient);
             } else {
                 partialDerivative.zero();
             }
@@ -114,56 +243,126 @@ namespace iDynTree {
             return true;
         }
 
-        bool QuadraticCost::costFirstPartialDerivativeWRTControl(double /*time*/,
-                                                          const iDynTree::VectorDynSize& /*state*/,
-                                                          const iDynTree::VectorDynSize& control,
-                                                          iDynTree::VectorDynSize& partialDerivative)
+        bool QuadraticCost::costFirstPartialDerivativeWRTControl(double time,
+                                                                 const iDynTree::VectorDynSize& /*state*/,
+                                                                 const iDynTree::VectorDynSize& control,
+                                                                 iDynTree::VectorDynSize& partialDerivative)
         {
             partialDerivative.resize(control.size());
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
-                if (m_controlHessian.rows() != control.size()) {
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", "The specified control hessian matrix dimensions do not match the control dimension.");
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
                     return false;
                 }
-                toEigen(partialDerivative) = m_controlCostScale * toEigen(m_controlHessian) * toEigen(control) + toEigen(m_controlGradient);
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingControlGradient->getObject(time, isValid);
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                toEigen(partialDerivative) = m_controlCostScale * toEigen(hessian) * toEigen(control) + toEigen(gradient);
             } else {
                 partialDerivative.zero();
             }
             return true;
         }
 
-        bool QuadraticCost::costSecondPartialDerivativeWRTState(double /*time*/,
-                                                         const iDynTree::VectorDynSize& state,
-                                                         const iDynTree::VectorDynSize& /*control*/,
-                                                         iDynTree::MatrixDynSize& partialDerivative)
+        bool QuadraticCost::costSecondPartialDerivativeWRTState(double time,
+                                                                const iDynTree::VectorDynSize& state,
+                                                                const iDynTree::VectorDynSize& /*control*/,
+                                                                iDynTree::MatrixDynSize& partialDerivative)
         {
             partialDerivative.resize(state.size(), state.size());
             if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
-                if (m_stateHessian.rows() != state.size()) {
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", "The specified state hessian matrix dimensions do not match the state dimension.");
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
                     return false;
                 }
-                toEigen(partialDerivative) = m_stateCostScale * toEigen(m_stateHessian);
+
+                toEigen(partialDerivative) = m_stateCostScale * toEigen(hessian);
             } else {
                 partialDerivative.zero();
             }
             return true;
         }
 
-        bool QuadraticCost::costSecondPartialDerivativeWRTControl(double /*time*/,
-                                                           const iDynTree::VectorDynSize& /*state*/,
-                                                           const iDynTree::VectorDynSize& control,
-                                                           iDynTree::MatrixDynSize& partialDerivative)
+        bool QuadraticCost::costSecondPartialDerivativeWRTControl(double time,
+                                                                  const iDynTree::VectorDynSize& /*state*/,
+                                                                  const iDynTree::VectorDynSize& control,
+                                                                  iDynTree::MatrixDynSize& partialDerivative)
         {
             partialDerivative.resize(control.size(), control.size());
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
-                if (m_controlHessian.rows() != control.size()) {
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", "The specified control hessian matrix dimensions do not match the control dimension.");
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
                     return false;
                 }
-                toEigen(partialDerivative) = m_controlCostScale * toEigen(m_controlHessian);
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
+                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                toEigen(partialDerivative) = m_controlCostScale * toEigen(hessian);
             } else {
                 partialDerivative.zero();
             }
diff --git a/src/optimalcontrol/src/TimeVaryingObject.cpp b/src/optimalcontrol/src/TimeVaryingObject.cpp
new file mode 100644
index 00000000000..b99c28a0562
--- /dev/null
+++ b/src/optimalcontrol/src/TimeVaryingObject.cpp
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/TimeVaryingObject.h>
+
+namespace iDynTree {
+    namespace optimalcontrol {
+
+        TimeInvariantVector::TimeInvariantVector()
+        { }
+
+        TimeInvariantVector::TimeInvariantVector(const VectorDynSize &timeInvariantVector)
+            : m_timeInvariantVector(timeInvariantVector)
+        { }
+
+        VectorDynSize &TimeInvariantVector::get()
+        {
+            return m_timeInvariantVector;
+        }
+
+        const VectorDynSize &TimeInvariantVector::getObject(double /*time*/, bool &isValid)
+        {
+            isValid = true;
+            return m_timeInvariantVector;
+        }
+
+        TimeInvariantMatrix::TimeInvariantMatrix()
+        { }
+
+        TimeInvariantMatrix::TimeInvariantMatrix(const MatrixDynSize &timeInvariantMatrix)
+            : m_timeInvariantMatrix(timeInvariantMatrix)
+        { }
+
+        MatrixDynSize &TimeInvariantMatrix::get()
+        {
+            return m_timeInvariantMatrix;
+        }
+
+        const MatrixDynSize &TimeInvariantMatrix::getObject(double /*time*/, bool &isValid)
+        {
+            isValid = true;
+            return m_timeInvariantMatrix;
+        }
+    }
+}
diff --git a/src/optimalcontrol/tests/IntegratorsTest.cpp b/src/optimalcontrol/tests/IntegratorsTest.cpp
index bbd46f9f0a6..abec4afba51 100644
--- a/src/optimalcontrol/tests/IntegratorsTest.cpp
+++ b/src/optimalcontrol/tests/IntegratorsTest.cpp
@@ -24,6 +24,7 @@
 #include <memory>
 #include <cmath>
 #include <iostream>
+#include <ctime>
 
 using namespace iDynTree;
 using namespace iDynTree::optimalcontrol;
@@ -57,6 +58,21 @@ class TestEquation1 : public DynamicalSystem{
         return m_initialConditions;
     }
 
+    bool  dynamicsStateFirstDerivative(const VectorDynSize& state,
+                                                        double time,
+                                                        MatrixDynSize& dynamicsDerivative) override
+    {
+        dynamicsDerivative.resize(1,1);
+        dynamicsDerivative(0,0) =m_lambda;
+        return true; }
+
+    bool dynamicsControlFirstDerivative(const VectorDynSize& state,
+                                                         double time,
+                                                         MatrixDynSize& dynamicsDerivative) override
+    {
+        dynamicsDerivative.resize(0,0);
+        return true; }
+
 };
 
 class TestEquation2 : public DynamicalSystem{
@@ -260,5 +276,31 @@ int main(){
     relTol = 5E-2;
     IntegratorTest3(FE_3);
 
+
+    iDynTree::VectorDynSize v1(1), v2(1), v3;
+    iDynTree::getRandomVector(v1);
+    iDynTree::getRandomVector(v2);
+    std::vector<VectorDynSize> c1(2), c2(2), c3;
+    c1[0] = v1;
+    c1[1] = v2;
+
+    std::shared_ptr<Integrator> ptr = std::make_shared<ForwardEuler>(dynamicalSystem);
+    ForwardEuler direct(dynamicalSystem);
+
+    clock_t initT, endT;
+    initT = clock();
+    for (int i = 0; i < 10000; ++i) {
+        ASSERT_IS_TRUE(ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
+    }
+    endT = clock();
+    std::cerr << "Elapsed time (ptr): " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+
+    initT = clock();
+    for (int i = 0; i < 10000; ++i) {
+        ASSERT_IS_TRUE(direct.evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
+    }
+    endT = clock();
+    std::cerr << "Elapsed time (direct): " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+
     return EXIT_SUCCESS;
 }

From c5ae2bfc19555c8c4892a05a354519afa1f34d4c Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 14:40:31 +0200
Subject: [PATCH 011/194] Modified the L2Norm cost and the quadratic cost to
 allow correct evaluation of norm.

When translating a L2Norm cost into a quadratic cost, the constant part given by the square of the reference value can be omitted. On the other end, for applications when it is important to have the correct value of the 2-norm, this constant part can be computed and added to the cost.
---
 .../include/iDynTree/L2NormCost.h             |   3 +
 .../include/iDynTree/QuadraticCost.h          |  10 +-
 .../include/iDynTree/TimeVaryingObject.h      |  14 ++
 src/optimalcontrol/src/L2NormCost.cpp         | 212 ++++++++++++------
 src/optimalcontrol/src/QuadraticCost.cpp      |  86 ++++++-
 src/optimalcontrol/src/TimeVaryingObject.cpp  |  23 +-
 6 files changed, 276 insertions(+), 72 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index f40f1b2e742..ad8cc4e5e15 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -41,6 +41,7 @@ namespace iDynTree {
 
             using QuadraticCost::setStateCost;  //avoid the user to access these methods to avoid confusion
             using QuadraticCost::setControlCost;
+            using QuadraticCost::setCostBias;
 
         public:
 
@@ -52,6 +53,8 @@ namespace iDynTree {
 
             ~L2NormCost();
 
+            void computeConstantPart(bool addItToTheCost = true); //by default the constant part is not added since it adds computational burden. Use it if you need the cost evaluation to be exactly equal to an L2 norm
+
             bool setStateWeight(const MatrixDynSize& stateWeights);
 
             bool setStateDesiredPoint(const VectorDynSize& desiredPoint);
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index ba353f1dabf..20246a5885e 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -49,10 +49,15 @@ namespace iDynTree {
            bool setControlCost(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> timeVaryingControlHessian,
                                std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> timeVaryingControlGradient);
 
+           bool setCostBias(double stateCostBias, double controlCostBias);
+
+           bool setCostBias(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingStateCostBias,
+                            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingControlCostBias);
+
            virtual bool costEvaluation(double time,
                                        const iDynTree::VectorDynSize& state,
                                        const iDynTree::VectorDynSize& control,
-                                       double& costValue) override;//0.5 xT H x + gx
+                                       double& costValue) override;//0.5 xT H x + gx + c
 
            virtual bool costFirstPartialDerivativeWRTState(double time,
                                                            const iDynTree::VectorDynSize& state,
@@ -83,8 +88,11 @@ namespace iDynTree {
        protected:
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingStateHessian;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingStateGradient;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingStateCostBias;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingControlCostBias;
+
            double m_stateCostScale, m_controlCostScale;
        };
     }
diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
index 222c7e9aed2..bb65ba453f7 100644
--- a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
+++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
@@ -44,6 +44,8 @@ namespace iDynTree {
 
         typedef TimeVaryingObject<MatrixDynSize> TimeVaryingMatrix;
 
+        typedef TimeVaryingObject<double> TimeVaryingDouble;
+
         /**
          * @warning This class is still in active development, and so API interface can change between iDynTree versions.
          * \ingroup iDynTreeExperimental
@@ -72,6 +74,18 @@ namespace iDynTree {
             virtual const MatrixDynSize& getObject(double time, bool &isValid) override;
         };
 
+        class TimeInvariantDouble : public TimeVaryingDouble {
+            double m_timeInvariantDouble;
+        public:
+            TimeInvariantDouble();
+
+            TimeInvariantDouble(double timeInvariantDouble);
+
+            double& get();
+
+            virtual const double& getObject(double time, bool &isValid) override;
+        };
+
     }
 }
 
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 7cbe7f056f7..bceb24fe32c 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -27,96 +27,154 @@
 namespace iDynTree {
     namespace optimalcontrol {
 
-    class TimeVaryingGradient : public TimeVaryingVector {
-        MatrixDynSize m_selectorMatrix, m_weightMatrix;
-        MatrixDynSize m_subMatrix; //weightMatrix times selector
-        std::shared_ptr<TimeVaryingVector> m_desiredTrajectory;
-        VectorDynSize m_outputVector;
-    public:
-        TimeVaryingGradient(const MatrixDynSize& selectorMatrix)
-        : m_selectorMatrix(selectorMatrix)
-        , m_desiredTrajectory(nullptr)
-        {
-            m_weightMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.rows());
-            toEigen(m_weightMatrix).setIdentity();
-            m_subMatrix = /*m_pimpl->stateWeight * */ m_selectorMatrix;
-            m_outputVector.resize(m_selectorMatrix.cols());
-            m_outputVector.zero();
-        }
+        class TimeVaryingGradient : public TimeVaryingVector {
+            MatrixDynSize m_selectorMatrix, m_weightMatrix;
+            MatrixDynSize m_subMatrix; //weightMatrix times selector
+            std::shared_ptr<TimeVaryingVector> m_desiredTrajectory;
+            VectorDynSize m_outputVector;
+        public:
+            TimeVaryingGradient(const MatrixDynSize& selectorMatrix)
+            : m_selectorMatrix(selectorMatrix)
+            , m_desiredTrajectory(nullptr)
+            {
+                m_weightMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.rows());
+                toEigen(m_weightMatrix).setIdentity();
+                m_subMatrix = /*m_pimpl->stateWeight * */ m_selectorMatrix;
+                m_outputVector.resize(m_selectorMatrix.cols());
+                m_outputVector.zero();
+            }
 
-        ~TimeVaryingGradient() override;
+            ~TimeVaryingGradient() override;
 
-        bool setDesiredTrajectory(std::shared_ptr<TimeVaryingVector> desiredTrajectory) {
-            if (!desiredTrajectory) {
-                reportError("TimeVaryingGradient", "desiredTrajectory", "Empty desired trajectory pointer.");
-                return false;
+            bool setDesiredTrajectory(std::shared_ptr<TimeVaryingVector> desiredTrajectory) {
+                if (!desiredTrajectory) {
+                    reportError("TimeVaryingGradient", "desiredTrajectory", "Empty desired trajectory pointer.");
+                    return false;
+                }
+                m_desiredTrajectory = desiredTrajectory;
+                return true;
             }
-            m_desiredTrajectory = desiredTrajectory;
-            return true;
-        }
 
-        bool setWeightMatrix(const MatrixDynSize &weights)
-        {
-            if (weights.rows() != weights.cols()) {
-                reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix is supposed to be squared.");
-                return false;
-            }
+            bool setWeightMatrix(const MatrixDynSize &weights)
+            {
+                if (weights.rows() != weights.cols()) {
+                    reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix is supposed to be squared.");
+                    return false;
+                }
 
-            if (weights.cols() != m_selectorMatrix.rows()) {
-                reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix dimensions do not match those of the specified selector.");
-                return false;
+                if (weights.cols() != m_selectorMatrix.rows()) {
+                    reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix dimensions do not match those of the specified selector.");
+                    return false;
+                }
+
+                m_weightMatrix = weights;
+                toEigen(m_subMatrix) = toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+
+                return true;
             }
 
-            m_weightMatrix = weights;
-            toEigen(m_subMatrix) = toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+            virtual const VectorDynSize& getObject(double time, bool &isValid) override {
+                if (!m_desiredTrajectory) {
+                    isValid = true;
+                    return m_outputVector; //should be zero from the initialization
+                }
+                bool ok = false;
+                const VectorDynSize &desiredPoint = m_desiredTrajectory->getObject(time, ok);
 
-            return true;
-        }
+                if (!ok) {
+                    isValid = false;
+                    m_outputVector.zero();
+                    return m_outputVector;
+                }
+
+                if (desiredPoint.size() != m_subMatrix.rows()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The specified desired point at time: " << time << " has size not matching the specified selector.";
+                    reportError("TimeVaryingGradient", "getObject", errorMsg.str().c_str());
+                    isValid = false;
+                    m_outputVector.zero();
+                    return m_outputVector;
+                }
+
+                toEigen(m_outputVector) = -1.0 * toEigen(desiredPoint).transpose() * toEigen(m_subMatrix);
 
-        virtual const VectorDynSize& getObject(double time, bool &isValid) override {
-            if (!m_desiredTrajectory) {
                 isValid = true;
-                return m_outputVector; //should be zero from the initialization
+                return m_outputVector;
             }
-            bool ok = false;
-            const VectorDynSize &desiredPoint = m_desiredTrajectory->getObject(time, ok);
 
-            if (!ok) {
-                isValid = false;
-                m_outputVector.zero();
-                return m_outputVector;
+            const MatrixDynSize& selector() {
+                return m_selectorMatrix;
             }
 
-            if (desiredPoint.size() != m_subMatrix.rows()) {
-                std::ostringstream errorMsg;
-                errorMsg << "The specified desired point at time: " << time << " has size not matching the specified selector.";
-                reportError("TimeVaryingGradient", "getObject", errorMsg.str().c_str());
-                isValid = false;
-                m_outputVector.zero();
-                return m_outputVector;
+            const MatrixDynSize& subMatrix() {
+                return m_subMatrix;
             }
 
-            toEigen(m_outputVector) = -1.0 * toEigen(desiredPoint).transpose() * toEigen(m_subMatrix);
+            const MatrixDynSize& weightMatrix() {
+                return m_weightMatrix;
+            }
 
-            isValid = true;
-            return m_outputVector;
-        }
+            std::shared_ptr<TimeVaryingVector> desiredTrajectory() {
+                return m_desiredTrajectory;
+            }
+        };
+        TimeVaryingGradient::~TimeVaryingGradient() {};
+
+        class TimeVaryingBias : public TimeVaryingDouble {
+            std::shared_ptr<TimeVaryingGradient> m_associatedGradient;
+            double m_output;
+        public:
+            TimeVaryingBias(std::shared_ptr<TimeVaryingGradient> associatedGradient)
+            :m_associatedGradient(associatedGradient)
+            { }
+
+            ~TimeVaryingBias() override;
+
+            virtual const double& getObject(double time, bool &isValid) override{
+                if (!(m_associatedGradient)) {
+                    isValid = false;
+                    m_output = 0.0;
+                    return m_output;
+                }
 
-        const MatrixDynSize& selector() {
-            return m_selectorMatrix;
-        }
+                if (!(m_associatedGradient->desiredTrajectory())) {
+                    isValid = true;
+                    m_output = 0.0;
+                    return m_output;
+                }
 
-        const MatrixDynSize& subMatrix() {
-            return m_subMatrix;
-        }
-    };
-    TimeVaryingGradient::~TimeVaryingGradient() {};
+                bool ok = false;
+                const VectorDynSize &desiredPoint = m_associatedGradient->desiredTrajectory()->getObject(time, ok);
+
+                if (!ok) {
+                    isValid = false;
+                    m_output = 0.0;
+                    return m_output;
+                }
+
+                if (desiredPoint.size() != m_associatedGradient->weightMatrix().rows()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The specified desired point at time: " << time << " has size not matching the specified weight matrix.";
+                    reportError("TimeVaryingBias", "getObject", errorMsg.str().c_str());
+                    isValid = false;
+                    m_output = 0.0;
+                    return m_output;
+                }
+
+                isValid = true;
+                m_output = 0.5 * toEigen(desiredPoint).transpose() * toEigen(m_associatedGradient->weightMatrix()) * toEigen(desiredPoint);
+                return m_output;
+            }
+        };
+        TimeVaryingBias::~TimeVaryingBias() { }
 
 
         class L2NormCost::L2NormCostImplementation {
             public:
-            std::shared_ptr<TimeVaryingGradient> stateGradient, controlGradient;
+            std::shared_ptr<TimeVaryingGradient> stateGradient = nullptr, controlGradient = nullptr;
             std::shared_ptr<TimeInvariantMatrix> stateHessian, controlHessian;
+            std::shared_ptr<TimeVaryingBias> stateCostBias, controlCostBias;
+            bool addConstantPart = false;
 
             void initialize(const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector) {
                 if ((stateSelector.rows() != 0) && (stateSelector.cols() != 0)) {
@@ -124,6 +182,7 @@ namespace iDynTree {
                     stateHessian = std::make_shared<TimeInvariantMatrix>();
                     stateHessian->get().resize(stateSelector.cols(), stateSelector.cols());
                     toEigen(stateHessian->get()) = toEigen(stateGradient->selector()).transpose() * toEigen(stateGradient->subMatrix());
+                    stateCostBias = std::make_shared<TimeVaryingBias>(stateGradient);
                 } else {
                     stateGradient = nullptr;
                 }
@@ -133,6 +192,7 @@ namespace iDynTree {
                     controlHessian = std::make_shared<TimeInvariantMatrix>();
                     controlHessian->get().resize(controlSelector.cols(), controlSelector.cols());
                     toEigen(controlHessian->get()) = toEigen(controlGradient->selector()).transpose() * toEigen(controlGradient->subMatrix());
+                    controlCostBias = std::make_shared<TimeVaryingBias>(controlGradient);
                 } else {
                     controlGradient = nullptr;
                 }
@@ -196,6 +256,26 @@ namespace iDynTree {
             }
         }
 
+        void L2NormCost::computeConstantPart(bool addItToTheCost)
+        {
+            m_pimpl->addConstantPart = addItToTheCost;
+            if (m_pimpl->addConstantPart) {
+                std::shared_ptr<TimeVaryingDouble> stateBias(new TimeInvariantDouble(0.0)), controlBias(new TimeInvariantDouble(0.0));
+                if (m_pimpl->stateGradient) {
+                    stateBias = m_pimpl->stateCostBias;
+                }
+
+                if (m_pimpl->controlGradient) {
+                    controlBias = m_pimpl->controlCostBias;
+                }
+
+                setCostBias(stateBias, controlBias);
+
+            } else {
+                setCostBias(0.0, 0.0);
+            }
+        }
+
         bool L2NormCost::setStateWeight(const MatrixDynSize &stateWeights)
         {
             if (stateWeights.rows() != stateWeights.cols()) {
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index 882d0bafc1b..f769b64c178 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -25,9 +25,16 @@ namespace iDynTree {
 
         QuadraticCost::QuadraticCost(const std::string &costName)
             : Cost(costName)
+            , m_timeVaryingStateHessian(nullptr)
+            , m_timeVaryingStateGradient(nullptr)
+            , m_timeVaryingControlHessian(nullptr)
+            , m_timeVaryingControlGradient(nullptr)
             , m_stateCostScale(0.0)
             , m_controlCostScale(0.0)
-        { }
+        {
+            m_timeVaryingStateCostBias = std::make_shared<TimeInvariantDouble>(0.0);
+            m_timeVaryingControlCostBias = std::make_shared<TimeInvariantDouble>(0.0);
+        }
 
         QuadraticCost::~QuadraticCost()
         { }
@@ -106,6 +113,31 @@ namespace iDynTree {
             return true;
         }
 
+        bool QuadraticCost::setCostBias(double stateCostBias, double controlCostBias)
+        {
+            m_timeVaryingStateCostBias.reset(new TimeInvariantDouble(stateCostBias));
+            m_timeVaryingControlCostBias.reset(new TimeInvariantDouble(controlCostBias));
+            return true;
+        }
+
+        bool QuadraticCost::setCostBias(std::shared_ptr<TimeVaryingDouble> timeVaryingStateCostBias,
+                                        std::shared_ptr<TimeVaryingDouble> timeVaryingControlCostBias)
+        {
+            if (!timeVaryingStateCostBias) {
+                reportError("QuadraticCost", "addCostBias", "The timeVaryingStateCostBias pointer is empty.");
+                return false;
+            }
+
+            if (!timeVaryingControlCostBias) {
+                reportError("QuadraticCost", "addCostBias", "The timeVaryingControlCostBias pointer is empty.");
+                return false;
+            }
+
+            m_timeVaryingStateCostBias = timeVaryingStateCostBias;
+            m_timeVaryingControlCostBias = timeVaryingControlCostBias;
+            return true;
+        }
+
         bool QuadraticCost::costEvaluation(double time,
                                            const iDynTree::VectorDynSize& state,
                                            const iDynTree::VectorDynSize& control,
@@ -141,6 +173,13 @@ namespace iDynTree {
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingStateGradient->getObject(time, isValid);
 
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state gradient at time: " << time << ".";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
                 if (hessian.rows() != gradient.size()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
@@ -148,7 +187,17 @@ namespace iDynTree {
                     return false;
                 }
 
-                stateCost = m_stateCostScale * 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0) + toEigen(gradient).transpose()*toEigen(state);
+                isValid = false;
+                double stateBias = m_timeVaryingStateCostBias->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state cost bias at time: " << time << ".";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                stateCost = m_stateCostScale * 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0) + toEigen(gradient).transpose()*toEigen(state) + stateBias;
             }
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
@@ -179,6 +228,13 @@ namespace iDynTree {
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingControlGradient->getObject(time, isValid);
 
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control gradient at time: " << time << ".";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
                 if (hessian.rows() != gradient.size()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
@@ -186,7 +242,17 @@ namespace iDynTree {
                     return false;
                 }
 
-                controlCost = m_controlCostScale * 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0) + toEigen(gradient).transpose()*toEigen(control);
+                isValid = false;
+                double controlBias = m_timeVaryingControlCostBias->getObject(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control cost bias at time: " << time << ".";
+                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                controlCost = m_controlCostScale * 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0) + toEigen(gradient).transpose()*toEigen(control) + controlBias;
             }
 
             costValue = stateCost + controlCost;
@@ -228,6 +294,13 @@ namespace iDynTree {
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingStateGradient->getObject(time, isValid);
 
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state gradient at time: " << time << ".";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
                 if (hessian.rows() != gradient.size()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
@@ -278,6 +351,13 @@ namespace iDynTree {
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingControlGradient->getObject(time, isValid);
 
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control gradient at time: " << time << ".";
+                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
                 if (hessian.rows() != gradient.size()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
diff --git a/src/optimalcontrol/src/TimeVaryingObject.cpp b/src/optimalcontrol/src/TimeVaryingObject.cpp
index b99c28a0562..67b0df5c343 100644
--- a/src/optimalcontrol/src/TimeVaryingObject.cpp
+++ b/src/optimalcontrol/src/TimeVaryingObject.cpp
@@ -23,7 +23,7 @@ namespace iDynTree {
         { }
 
         TimeInvariantVector::TimeInvariantVector(const VectorDynSize &timeInvariantVector)
-            : m_timeInvariantVector(timeInvariantVector)
+        : m_timeInvariantVector(timeInvariantVector)
         { }
 
         VectorDynSize &TimeInvariantVector::get()
@@ -41,7 +41,7 @@ namespace iDynTree {
         { }
 
         TimeInvariantMatrix::TimeInvariantMatrix(const MatrixDynSize &timeInvariantMatrix)
-            : m_timeInvariantMatrix(timeInvariantMatrix)
+        : m_timeInvariantMatrix(timeInvariantMatrix)
         { }
 
         MatrixDynSize &TimeInvariantMatrix::get()
@@ -54,5 +54,24 @@ namespace iDynTree {
             isValid = true;
             return m_timeInvariantMatrix;
         }
+
+        TimeInvariantDouble::TimeInvariantDouble()
+        { }
+
+        TimeInvariantDouble::TimeInvariantDouble(double timeInvariantDouble)
+            : m_timeInvariantDouble(timeInvariantDouble)
+        { }
+
+        double &TimeInvariantDouble::get()
+        {
+            return m_timeInvariantDouble;
+        }
+
+        const double &TimeInvariantDouble::getObject(double /*time*/, bool &isValid)
+        {
+            isValid = true;
+            return m_timeInvariantDouble;
+        }
+
     }
 }

From af51bfc60da7c9eba2d3124c7904234b1206fe1e Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 14:43:30 +0200
Subject: [PATCH 012/194] Added benchmark on virtual functions evaluation.

---
 src/optimalcontrol/tests/IntegratorsTest.cpp | 24 ++++++++++++--------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/src/optimalcontrol/tests/IntegratorsTest.cpp b/src/optimalcontrol/tests/IntegratorsTest.cpp
index abec4afba51..d0accdf1641 100644
--- a/src/optimalcontrol/tests/IntegratorsTest.cpp
+++ b/src/optimalcontrol/tests/IntegratorsTest.cpp
@@ -287,20 +287,24 @@ int main(){
     std::shared_ptr<Integrator> ptr = std::make_shared<ForwardEuler>(dynamicalSystem);
     ForwardEuler direct(dynamicalSystem);
 
-    clock_t initT, endT;
-    initT = clock();
-    for (int i = 0; i < 10000; ++i) {
-        ASSERT_IS_TRUE(ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
-    }
-    endT = clock();
-    std::cerr << "Elapsed time (ptr): " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+    clock_t initT, endT, sumDirect = 0, sumPtr = 0;
+
+    ASSERT_IS_TRUE(direct.evaluateCollocationConstraint(0.0, c1, c2, dT, v3)); //for eventual memory allocation
+    ASSERT_IS_TRUE(ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
 
-    initT = clock();
     for (int i = 0; i < 10000; ++i) {
+        initT = clock();
         ASSERT_IS_TRUE(direct.evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
+        endT = clock();
+        sumDirect += (endT-initT);
+
+        initT = clock();
+        ASSERT_IS_TRUE(ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
+        endT = clock();
+        sumPtr += (endT-initT);
     }
-    endT = clock();
-    std::cerr << "Elapsed time (direct): " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+    std::cerr << "Elapsed time (direct): " <<  static_cast<double>(sumDirect) *1000 / CLOCKS_PER_SEC <<" ms."<<std::endl;
+    std::cerr << "Elapsed time (ptr): " <<  static_cast<double>(sumPtr) *1000 / CLOCKS_PER_SEC <<" ms."<<std::endl; //Multiplying first to avoid decimation issues
 
     return EXIT_SUCCESS;
 }

From 0ccca9a6ae9dc404bfee6dbb90f8d56738e1117d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 14:44:05 +0200
Subject: [PATCH 013/194] Implemented linear constraints and linear system.

---
 .../include/iDynTree/LinearConstraint.h       |  14 +-
 .../include/iDynTree/LinearSystem.h           |  43 ++---
 src/optimalcontrol/src/LinearConstraint.cpp   |  99 ++++++++++--
 src/optimalcontrol/src/LinearSystem.cpp       | 152 +++++++++++++-----
 4 files changed, 220 insertions(+), 88 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
index 7411bfe4787..b33bfee8846 100644
--- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h
+++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
@@ -18,7 +18,7 @@
 #define IDYNTREE_OPTIMALCONTROL_LINEARCONSTRAINT_H
 
 #include <iDynTree/Constraint.h>
-
+#include <string>
 #include <iDynTree/Core/MatrixDynSize.h>
 
 namespace iDynTree {
@@ -40,12 +40,18 @@ namespace iDynTree {
         : public Constraint {
         public:
 
-            virtual ~LinearConstraint();
+            LinearConstraint(size_t size, const std::string name);
+
+            virtual ~LinearConstraint() override;
+
+            bool setStateConstraintMatrix(const MatrixDynSize& constraintMatrix);
+
+            bool setControlConstraintMatrix(const MatrixDynSize& constraintMatrix);
 
             virtual bool evaluateConstraint(double time,
                                             const VectorDynSize& state,
                                             const VectorDynSize& control,
-                                            VectorDynSize& constraint) override;
+                                            VectorDynSize& constraint) override;// lu <= [Ax, Au][x;u] <= lU
 
             virtual bool constraintJacobianWRTState(double time,
                                                     const VectorDynSize& state,
@@ -58,8 +64,10 @@ namespace iDynTree {
                                                       MatrixDynSize& jacobian) override;
 
         private:
+            bool m_constrainsState, m_constrainsControl;
             iDynTree::MatrixDynSize m_stateConstraintMatrix;
             iDynTree::MatrixDynSize m_controlConstraintMatrix;
+            iDynTree::VectorDynSize m_stateConstraintsBuffer, m_controlConstraintsBuffer;
         };
 
     }
diff --git a/src/optimalcontrol/include/iDynTree/LinearSystem.h b/src/optimalcontrol/include/iDynTree/LinearSystem.h
index abaf5397430..43844df3895 100644
--- a/src/optimalcontrol/include/iDynTree/LinearSystem.h
+++ b/src/optimalcontrol/include/iDynTree/LinearSystem.h
@@ -18,18 +18,15 @@
 #define IDYNTREE_OPTIMALCONTROL_LINEARSYSTEM_H
 
 #include <iDynTree/DynamicalSystem.h>
+#include <iDynTree/TimeVaryingObject.h>
+#include <memory>
 
-#include <vector>
 namespace iDynTree {
 
     class MatrixDynSize;
 
     namespace optimalcontrol {
 
-        class Controller;
-        // Add somewhere the idea of output. In case of time varying system it may share the same parameter of
-        // the A or B matrix
-
         /**
          * @warning This class is still in active development, and so API interface can change between iDynTree versions.
          * \ingroup iDynTreeExperimental
@@ -40,37 +37,21 @@ namespace iDynTree {
 
         public:
 
-            LinearSystem(size_t stateSize,
-                         size_t controlSize,
-                         bool isTimeVarying);
+            LinearSystem(size_t stateSize, size_t controlSize);
 
             LinearSystem(const LinearSystem& other) = delete;
 
-            ~LinearSystem();
-
-            bool isTimeVarying();
+            ~LinearSystem() override;
 
             // time invariant system: single matrix
-            void setConstantStateMatrix(const iDynTree::MatrixDynSize&);
-            void setConstantControlMatrix(const iDynTree::MatrixDynSize&);
-
-            // time varying cases:
-            // 1) preallocate matrices [initialTime, finalTime)
-            void setStateMatrixForTimeRange(const iDynTree::MatrixDynSize&, double initialTime, double finalTime);
-            //or
-            void setStateMatricesForTimeRanges(std::vector<const iDynTree::MatrixDynSize&>,
-                                               std::vector<std::pair<double, double> >);
-
-            // 2) lambda which will be called at runtime (how this will work with eventual derivatives?)
-            // Is this feasible?
-            // Ideally I would call the lambda as  (sender, time)-> iDynTree::MatrixDynSize&
-            // But if the lambda captures variable it cannot be passed as function pointer.
-            // Alternatevely, I can do a C-style call, like registering a function pointer of time
-            // iDynTree::MatrixDynSize& (*stateCallback)(const LinearSystem& sender, double time, void* context)
-            // context can be anything, only the caller knows about it.
-
-            iDynTree::MatrixDynSize& stateMatrix(double time) const;
-            iDynTree::MatrixDynSize& controlMatrix(double time) const;
+            bool setStateMatrix(const iDynTree::MatrixDynSize& stateMatrix);
+
+            bool setControlMatrix(const iDynTree::MatrixDynSize& controlMatrix);
+
+            //time variant case
+            bool setStateMatrix(std::shared_ptr<TimeVaryingMatrix> stateMatrix);
+
+            bool setControlMatrix(std::shared_ptr<TimeVaryingMatrix> controlMatrix);
 
             virtual bool dynamics(const VectorDynSize& state,
                                   double time,
diff --git a/src/optimalcontrol/src/LinearConstraint.cpp b/src/optimalcontrol/src/LinearConstraint.cpp
index afc981fc57e..25dee96a939 100644
--- a/src/optimalcontrol/src/LinearConstraint.cpp
+++ b/src/optimalcontrol/src/LinearConstraint.cpp
@@ -23,42 +23,119 @@
 namespace iDynTree {
     namespace optimalcontrol {
 
+        LinearConstraint::LinearConstraint(size_t size, const std::string name)
+        : Constraint(size, name)
+        , m_constrainsState(false)
+        , m_constrainsControl(false)
+        , m_stateConstraintsBuffer(static_cast<unsigned int>(size))
+        , m_controlConstraintsBuffer(static_cast<unsigned int>(size))
+        {
+            m_stateConstraintsBuffer.zero();
+            m_controlConstraintsBuffer.zero();
+        }
+
         LinearConstraint::~LinearConstraint() {}
 
+        bool LinearConstraint::setStateConstraintMatrix(const MatrixDynSize &constraintMatrix)
+        {
+            if (constraintMatrix.rows() != constraintSize()) {
+                reportError("LinearConstraint", "setStateConstraintMatrix", "The number of rows of the constraintMatrix is different from the specified constraint size.");
+                return false;
+            }
+
+            m_stateConstraintMatrix = constraintMatrix;
+            m_constrainsState = true;
+
+            return true;
+        }
+
+        bool LinearConstraint::setControlConstraintMatrix(const MatrixDynSize &constraintMatrix)
+        {
+            if (constraintMatrix.rows() != constraintSize()) {
+                reportError("LinearConstraint", "setControlConstraintMatrix", "The number of rows of the constraintMatrix is different from the specified constraint size.");
+                return false;
+            }
+
+            m_controlConstraintMatrix = constraintMatrix;
+            m_constrainsControl = true;
+
+            return true;
+        }
+
+
         bool LinearConstraint::evaluateConstraint(double time,
                                                   const VectorDynSize& state,
                                                   const VectorDynSize& control,
                                                   VectorDynSize& constraint)
         {
+            if (m_constrainsState) {
+                if (m_stateConstraintMatrix.cols() != state.size()) {
+                    reportError("LinearConstraint", "evaluateConstraint", "The dimension of the specified constraint matrix and the state do not match.");
+                    return false;
+                }
+                iDynTree::iDynTreeEigenMatrixMap stateConstraint = iDynTree::toEigen(m_stateConstraintMatrix);
+
+                iDynTree::toEigen(m_stateConstraintsBuffer) = stateConstraint * iDynTree::toEigen(state);
+            }
 
-             iDynTree::iDynTreeEigenMatrixMap stateConstraint = iDynTree::toEigen(m_stateConstraintMatrix);
-             iDynTree::iDynTreeEigenMatrixMap controlConstraint = iDynTree::toEigen(m_controlConstraintMatrix);
 
-            assert(stateConstraint.rows() + controlConstraint.rows() == constraintSize());
+            if (m_constrainsControl) {
+                if (m_controlConstraintMatrix.cols() != control.size()) {
+                    reportError("LinearConstraint", "evaluateConstraint", "The dimension of the specified constraint matrix and the control do not match.");
+                    return false;
+                }
 
-            iDynTree::toEigen(constraint).head(stateConstraint.rows()) = stateConstraint * iDynTree::toEigen(state);
-            iDynTree::toEigen(constraint).tail(controlConstraint.rows()) = controlConstraint * iDynTree::toEigen(control);
+                iDynTree::iDynTreeEigenMatrixMap controlConstraint = iDynTree::toEigen(m_controlConstraintMatrix);
+                iDynTree::toEigen(m_controlConstraintsBuffer) = controlConstraint * iDynTree::toEigen(control);
+            }
 
+            constraint.resize(static_cast<unsigned int>(constraintSize()));
+            toEigen(constraint) = iDynTree::toEigen(m_stateConstraintsBuffer) + iDynTree::toEigen(m_controlConstraintsBuffer); //the buffers are zero if not constrained
 
             return true;
 
         }
 
-        bool LinearConstraint::constraintJacobianWRTState(double time,
+        bool LinearConstraint::constraintJacobianWRTState(double /*time*/,
                                                 const VectorDynSize& state,
-                                                const VectorDynSize& control,
+                                                const VectorDynSize& /*control*/,
                                                 MatrixDynSize& jacobian)
         {
-            iDynTree::toEigen(jacobian) = iDynTree::toEigen(m_stateConstraintMatrix);
+            if (m_constrainsState) {
+                if (m_stateConstraintMatrix.cols() != state.size()) {
+                    reportError("LinearConstraint", "constraintJacobianWRTState", "The dimension of the specified constraint matrix and the state do not match.");
+                    return false;
+                }
+            } else {
+                if ((m_stateConstraintMatrix.rows() != static_cast<unsigned int>(constraintSize())) ||
+                        (m_stateConstraintMatrix.cols() != state.size())) {
+                    m_stateConstraintMatrix.resize(static_cast<unsigned int>(constraintSize()), state.size());
+                    m_stateConstraintMatrix.zero();
+                }
+            }
+            jacobian = m_stateConstraintMatrix;
+
             return true;
         }
 
-        bool LinearConstraint::constraintJacobianWRTControl(double time,
-                                                  const VectorDynSize& state,
+        bool LinearConstraint::constraintJacobianWRTControl(double /*time*/,
+                                                  const VectorDynSize& /*state*/,
                                                   const VectorDynSize& control,
                                                   MatrixDynSize& jacobian)
         {
-            iDynTree::toEigen(jacobian) = iDynTree::toEigen(m_controlConstraintMatrix);
+            if (m_constrainsControl) {
+                if (m_controlConstraintMatrix.cols() != control.size()) {
+                    reportError("LinearConstraint", "constraintJacobianWRTControl", "The dimension of the specified constraint matrix and the control do not match.");
+                    return false;
+                }
+            } else {
+                if ((m_controlConstraintMatrix.rows() != static_cast<unsigned int>(constraintSize())) ||
+                        (m_controlConstraintMatrix.cols() != control.size())) {
+                    m_controlConstraintMatrix.resize(static_cast<unsigned int>(constraintSize()), control.size());
+                    m_controlConstraintMatrix.zero();
+                }
+            }
+            jacobian = m_controlConstraintMatrix;
             return true;
         }
 
diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp
index 601706b4fa3..640b40a4084 100644
--- a/src/optimalcontrol/src/LinearSystem.cpp
+++ b/src/optimalcontrol/src/LinearSystem.cpp
@@ -15,7 +15,6 @@
  */
 
 #include <iDynTree/LinearSystem.h>
-#include <iDynTree/Controller.h>
 
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
@@ -31,32 +30,26 @@ namespace iDynTree {
         class LinearSystem::LinearSystemPimpl
         {
         public:
-            bool timeVarying;
-            size_t stateSpaceSize;
-            size_t controlSpaceSize;
-
-            iDynTree::MatrixDynSize stateMatrix;
-            iDynTree::MatrixDynSize controlMatrix;
-
-            Controller* controllerPointer;
-            iDynTree::VectorDynSize controlOutput;
-
-            LinearSystemPimpl()
-                :controllerPointer(nullptr)
-            {}
-
-            ~LinearSystemPimpl(){}
+            std::shared_ptr<TimeVaryingMatrix> stateMatrix;
+            std::shared_ptr<TimeVaryingMatrix> controlMatrix;
         };
 
 
         LinearSystem::LinearSystem(size_t stateSize,
-                                   size_t controlSize,
-                                   bool isTimeVarying)
+                                   size_t controlSize)
         : DynamicalSystem(stateSize, controlSize)
         , m_pimpl(new LinearSystemPimpl())
         {
             assert(m_pimpl);
-            m_pimpl->controlOutput.resize(controlSize);
+            std::shared_ptr<TimeInvariantMatrix> tempState = std::make_shared<TimeInvariantMatrix>();
+            tempState->get().resize(static_cast<unsigned int>(stateSize), static_cast<unsigned int>(stateSize));
+            tempState->get().zero();
+            m_pimpl->stateMatrix = tempState;
+
+            std::shared_ptr<TimeInvariantMatrix> tempControl = std::make_shared<TimeInvariantMatrix>();
+            tempControl->get().resize(static_cast<unsigned int>(stateSize), static_cast<unsigned int>(controlSize));
+            tempControl->get().zero();
+            m_pimpl->controlMatrix = tempControl;
         }
 
         LinearSystem::~LinearSystem()
@@ -67,27 +60,44 @@ namespace iDynTree {
             }
         }
 
-        bool LinearSystem::isTimeVarying()
+        bool LinearSystem::setStateMatrix(const MatrixDynSize &stateMatrix)
         {
-            assert(m_pimpl);
-            return m_pimpl->timeVarying;
+            if ((stateMatrix.rows() != stateSpaceSize()) || (stateMatrix.cols() != stateSpaceSize())) {
+                reportError("LinearSystem", "setStateMatrix", "The stateMatrix does not match the specified state space size.");
+                return false;
+            }
+            m_pimpl->stateMatrix.reset(new TimeInvariantMatrix(stateMatrix));
+            return true;
         }
 
-        iDynTree::MatrixDynSize& LinearSystem::stateMatrix(double time) const
+        bool LinearSystem::setControlMatrix(const MatrixDynSize &controlMatrix)
         {
-            assert(m_pimpl);
-            //if (!m_pimpl->timeVarying) {
-                return m_pimpl->stateMatrix;
-            //}
+            if ((controlMatrix.rows() != stateSpaceSize()) || (controlMatrix.cols() != controlSpaceSize())) {
+                reportError("LinearSystem", "setControlMatrix", "The controlMatrix does not match the specified state/control space size.");
+                return false;
+            }
+            m_pimpl->controlMatrix.reset(new TimeInvariantMatrix(controlMatrix));
+            return true;
+        }
 
+        bool LinearSystem::setStateMatrix(std::shared_ptr<TimeVaryingMatrix> stateMatrix)
+        {
+            if (!stateMatrix) {
+                reportError("LinearSystem", "setStateMatrix", "Empty stateMatrix pointer.");
+                return false;
+            }
+            m_pimpl->stateMatrix = stateMatrix;
+            return true;
         }
 
-        iDynTree::MatrixDynSize& LinearSystem::controlMatrix(double time) const
+        bool LinearSystem::setControlMatrix(std::shared_ptr<TimeVaryingMatrix> controlMatrix)
         {
-            assert(m_pimpl);
-            //if (!m_pimpl->timeVarying) {
-                return m_pimpl->controlMatrix;
-            //}
+            if (!controlMatrix) {
+                reportError("LinearSystem", "setControlMatrix", "Empty controlMatrix pointer.");
+                return false;
+            }
+            m_pimpl->controlMatrix = controlMatrix;
+            return true;
         }
 
 
@@ -95,35 +105,91 @@ namespace iDynTree {
                                     double time,
                                     VectorDynSize& stateDynamics)
         {
-            if(!m_pimpl->controllerPointer){
-                reportError("LinearSystem", "dynamics", "Controller not set.");
+            bool isValid = false;
+            const MatrixDynSize& stateMatrix = m_pimpl->stateMatrix->getObject(time, isValid);
+
+            if (!isValid) {
+                std::ostringstream errorMsg;
+                errorMsg << "Unable to retrieve a valid state matrix at time: " << time << ".";
+                reportError("LinearSystem", "dynamics", errorMsg.str().c_str());
                 return false;
             }
-            if(!m_pimpl->controllerPointer->setStateFeedback(time, state)){
-                reportError("LinearSystem", "dynamics", "Error while setting the feedback to the controller.");
+
+            if ((stateMatrix.rows() != stateSpaceSize()) || (stateMatrix.cols() != stateSpaceSize())) {
+                std::ostringstream errorMsg;
+                errorMsg << "The state matrix at time: " << time << " has dimensions not matching with the specified state space dimension.";
+                reportError("LinearSystem", "dynamics", errorMsg.str().c_str());
+                return false;
+            }
+
+            isValid = false;
+            const MatrixDynSize& controlMatrix = m_pimpl->controlMatrix->getObject(time, isValid);
+
+            if (!isValid) {
+                std::ostringstream errorMsg;
+                errorMsg << "Unable to retrieve a valid control matrix at time: " << time << ".";
+                reportError("LinearSystem", "dynamics", errorMsg.str().c_str());
                 return false;
             }
-            if(!m_pimpl->controllerPointer->doControl(m_pimpl->controlOutput)){
-                reportError("LinearSystem", "dynamics", "Error while retrieving the control input.");
+
+            if ((controlMatrix.rows() != stateSpaceSize()) || (controlMatrix.cols() != controlSpaceSize())) {
+                std::ostringstream errorMsg;
+                errorMsg << "The control matrix at time: " << time << " has dimensions not matching with the specified state/control space dimension.";
+                reportError("LinearSystem", "dynamics", errorMsg.str().c_str());
                 return false;
             }
-            iDynTree::toEigen(stateDynamics) = iDynTree::toEigen(stateMatrix(time)) * iDynTree::toEigen(state) + iDynTree::toEigen(controlMatrix(time)) * iDynTree::toEigen(m_pimpl->controlOutput);
+
+            iDynTree::toEigen(stateDynamics) = iDynTree::toEigen(stateMatrix) * iDynTree::toEigen(state) + iDynTree::toEigen(controlMatrix) * iDynTree::toEigen(controlInput());
             return true;
         }
 
-        bool LinearSystem::dynamicsStateFirstDerivative(const VectorDynSize& state,
+        bool LinearSystem::dynamicsStateFirstDerivative(const VectorDynSize& /*state*/,
                                                   double time,
                                                   MatrixDynSize& dynamicsDerivative)
         {
-            dynamicsDerivative = stateMatrix(time);
+            bool isValid = false;
+            const MatrixDynSize& stateMatrix = m_pimpl->stateMatrix->getObject(time, isValid);
+
+            if (!isValid) {
+                std::ostringstream errorMsg;
+                errorMsg << "Unable to retrieve a valid state matrix at time: " << time << ".";
+                reportError("LinearSystem", "dynamicsStateFirstDerivative", errorMsg.str().c_str());
+                return false;
+            }
+
+            if ((stateMatrix.rows() != stateSpaceSize()) || (stateMatrix.cols() != stateSpaceSize())) {
+                std::ostringstream errorMsg;
+                errorMsg << "The state matrix at time: " << time << " has dimensions not matching with the specified state space dimension.";
+                reportError("LinearSystem", "dynamicsStateFirstDerivative", errorMsg.str().c_str());
+                return false;
+            }
+
+            dynamicsDerivative = stateMatrix;
             return true;
         }
 
-        bool LinearSystem::dynamicsControlFirstDerivative(const VectorDynSize& state,
+        bool LinearSystem::dynamicsControlFirstDerivative(const VectorDynSize& /*state*/,
                                                     double time,
                                                     MatrixDynSize& dynamicsDerivative)
         {
-            dynamicsDerivative = controlMatrix(time);
+            bool isValid = false;
+            const MatrixDynSize& controlMatrix = m_pimpl->controlMatrix->getObject(time, isValid);
+
+            if (!isValid) {
+                std::ostringstream errorMsg;
+                errorMsg << "Unable to retrieve a valid control matrix at time: " << time << ".";
+                reportError("LinearSystem", "dynamicsControlFirstDerivative", errorMsg.str().c_str());
+                return false;
+            }
+
+            if ((controlMatrix.rows() != stateSpaceSize()) || (controlMatrix.cols() != controlSpaceSize())) {
+                std::ostringstream errorMsg;
+                errorMsg << "The control matrix at time: " << time << " has dimensions not matching with the specified state/control space dimension.";
+                reportError("LinearSystem", "dynamicsControlFirstDerivative", errorMsg.str().c_str());
+                return false;
+            }
+
+            dynamicsDerivative = controlMatrix;
             return true;
         }
 

From 7aec3fa35374a05e6cf3ca49b3c5bfd4e3b721ae Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 15:55:11 +0200
Subject: [PATCH 014/194] Using templates to define TimeInvariantObjects.

Renamed getObject to get and moved to private the attributes of QuadraticCost.
---
 .../include/iDynTree/QuadraticCost.h          |  3 +-
 .../include/iDynTree/TimeVaryingObject.h      | 42 ++++++---------
 src/optimalcontrol/src/L2NormCost.cpp         |  8 +--
 src/optimalcontrol/src/LinearSystem.cpp       |  8 +--
 src/optimalcontrol/src/QuadraticCost.cpp      | 24 ++++-----
 src/optimalcontrol/src/TimeVaryingObject.cpp  | 54 +++++--------------
 6 files changed, 50 insertions(+), 89 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index 20246a5885e..ea151c8fd05 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -85,14 +85,13 @@ namespace iDynTree {
                                                                    const iDynTree::VectorDynSize& control,
                                                                    iDynTree::MatrixDynSize& partialDerivative) override;
 
-       protected:
+       private:
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingStateHessian;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingStateGradient;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingStateCostBias;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingControlCostBias;
-
            double m_stateCostScale, m_controlCostScale;
        };
     }
diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
index bb65ba453f7..5a6b1d861e8 100644
--- a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
+++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
@@ -36,8 +36,7 @@ namespace iDynTree {
 
             virtual ~TimeVaryingObject() {}
 
-            virtual const Object& getObject(double time, bool &isValid) = 0;
-
+            virtual const Object& get(double time, bool &isValid) = 0;
         };
 
         typedef TimeVaryingObject<VectorDynSize> TimeVaryingVector;
@@ -50,42 +49,31 @@ namespace iDynTree {
          * @warning This class is still in active development, and so API interface can change between iDynTree versions.
          * \ingroup iDynTreeExperimental
          */
-        class TimeInvariantVector : public TimeVaryingVector {
-            VectorDynSize m_timeInvariantVector;
-        public:
-            TimeInvariantVector();
-
-            TimeInvariantVector(const VectorDynSize& timeInvariantVector);
 
-            VectorDynSize& get();
-
-            virtual const VectorDynSize& getObject(double time, bool &isValid) override;
-        };
-
-        class TimeInvariantMatrix : public TimeVaryingMatrix {
-            MatrixDynSize m_timeInvariantMatrix;
+        template <typename Object>
+        class TimeInvariantObject : public TimeVaryingObject<Object> {
+            Object m_invariantObject;
         public:
-            TimeInvariantMatrix();
+            TimeInvariantObject();
 
-            TimeInvariantMatrix(const MatrixDynSize& timeInvariantMatrix);
+            TimeInvariantObject(const Object& timeInvariantObject);
 
-            MatrixDynSize& get();
+            Object& get();
 
-            virtual const MatrixDynSize& getObject(double time, bool &isValid) override;
+            virtual const Object& get(double time, bool &isValid) final;
         };
 
-        class TimeInvariantDouble : public TimeVaryingDouble {
-            double m_timeInvariantDouble;
-        public:
-            TimeInvariantDouble();
+        extern template class TimeInvariantObject<double>;
 
-            TimeInvariantDouble(double timeInvariantDouble);
+        extern template class TimeInvariantObject<VectorDynSize>;
 
-            double& get();
+        extern template class TimeInvariantObject<MatrixDynSize>;
 
-            virtual const double& getObject(double time, bool &isValid) override;
-        };
+        typedef TimeInvariantObject<double> TimeInvariantDouble;
+
+        typedef TimeInvariantObject<VectorDynSize> TimeInvariantVector;
 
+        typedef TimeInvariantObject<MatrixDynSize> TimeInvariantMatrix;
     }
 }
 
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index bceb24fe32c..5e951422e6c 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -73,13 +73,13 @@ namespace iDynTree {
                 return true;
             }
 
-            virtual const VectorDynSize& getObject(double time, bool &isValid) override {
+            virtual const VectorDynSize& get(double time, bool &isValid) override {
                 if (!m_desiredTrajectory) {
                     isValid = true;
                     return m_outputVector; //should be zero from the initialization
                 }
                 bool ok = false;
-                const VectorDynSize &desiredPoint = m_desiredTrajectory->getObject(time, ok);
+                const VectorDynSize &desiredPoint = m_desiredTrajectory->get(time, ok);
 
                 if (!ok) {
                     isValid = false;
@@ -130,7 +130,7 @@ namespace iDynTree {
 
             ~TimeVaryingBias() override;
 
-            virtual const double& getObject(double time, bool &isValid) override{
+            virtual const double& get(double time, bool &isValid) override{
                 if (!(m_associatedGradient)) {
                     isValid = false;
                     m_output = 0.0;
@@ -144,7 +144,7 @@ namespace iDynTree {
                 }
 
                 bool ok = false;
-                const VectorDynSize &desiredPoint = m_associatedGradient->desiredTrajectory()->getObject(time, ok);
+                const VectorDynSize &desiredPoint = m_associatedGradient->desiredTrajectory()->get(time, ok);
 
                 if (!ok) {
                     isValid = false;
diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp
index 640b40a4084..e8f5237c886 100644
--- a/src/optimalcontrol/src/LinearSystem.cpp
+++ b/src/optimalcontrol/src/LinearSystem.cpp
@@ -106,7 +106,7 @@ namespace iDynTree {
                                     VectorDynSize& stateDynamics)
         {
             bool isValid = false;
-            const MatrixDynSize& stateMatrix = m_pimpl->stateMatrix->getObject(time, isValid);
+            const MatrixDynSize& stateMatrix = m_pimpl->stateMatrix->get(time, isValid);
 
             if (!isValid) {
                 std::ostringstream errorMsg;
@@ -123,7 +123,7 @@ namespace iDynTree {
             }
 
             isValid = false;
-            const MatrixDynSize& controlMatrix = m_pimpl->controlMatrix->getObject(time, isValid);
+            const MatrixDynSize& controlMatrix = m_pimpl->controlMatrix->get(time, isValid);
 
             if (!isValid) {
                 std::ostringstream errorMsg;
@@ -148,7 +148,7 @@ namespace iDynTree {
                                                   MatrixDynSize& dynamicsDerivative)
         {
             bool isValid = false;
-            const MatrixDynSize& stateMatrix = m_pimpl->stateMatrix->getObject(time, isValid);
+            const MatrixDynSize& stateMatrix = m_pimpl->stateMatrix->get(time, isValid);
 
             if (!isValid) {
                 std::ostringstream errorMsg;
@@ -173,7 +173,7 @@ namespace iDynTree {
                                                     MatrixDynSize& dynamicsDerivative)
         {
             bool isValid = false;
-            const MatrixDynSize& controlMatrix = m_pimpl->controlMatrix->getObject(time, isValid);
+            const MatrixDynSize& controlMatrix = m_pimpl->controlMatrix->get(time, isValid);
 
             if (!isValid) {
                 std::ostringstream errorMsg;
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index f769b64c178..17d943daf4e 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -147,7 +147,7 @@ namespace iDynTree {
 
             if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
                 bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingStateHessian->getObject(time, isValid);
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -171,7 +171,7 @@ namespace iDynTree {
                 }
 
                 isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingStateGradient->getObject(time, isValid);
+                const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -188,7 +188,7 @@ namespace iDynTree {
                 }
 
                 isValid = false;
-                double stateBias = m_timeVaryingStateCostBias->getObject(time, isValid);
+                double stateBias = m_timeVaryingStateCostBias->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -202,7 +202,7 @@ namespace iDynTree {
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
                 bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingControlHessian->getObject(time, isValid);
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -226,7 +226,7 @@ namespace iDynTree {
                 }
 
                 isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingControlGradient->getObject(time, isValid);
+                const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -243,7 +243,7 @@ namespace iDynTree {
                 }
 
                 isValid = false;
-                double controlBias = m_timeVaryingControlCostBias->getObject(time, isValid);
+                double controlBias = m_timeVaryingControlCostBias->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -268,7 +268,7 @@ namespace iDynTree {
 
             if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
                 bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingStateHessian->getObject(time, isValid);
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -292,7 +292,7 @@ namespace iDynTree {
                 }
 
                 isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingStateGradient->getObject(time, isValid);
+                const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -325,7 +325,7 @@ namespace iDynTree {
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
                 bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingControlHessian->getObject(time, isValid);
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -349,7 +349,7 @@ namespace iDynTree {
                 }
 
                 isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingControlGradient->getObject(time, isValid);
+                const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -380,7 +380,7 @@ namespace iDynTree {
             partialDerivative.resize(state.size(), state.size());
             if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
                 bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingStateHessian->getObject(time, isValid);
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
@@ -419,7 +419,7 @@ namespace iDynTree {
 
             if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
                 bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingControlHessian->getObject(time, isValid);
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
                 if (!isValid) {
                     std::ostringstream errorMsg;
diff --git a/src/optimalcontrol/src/TimeVaryingObject.cpp b/src/optimalcontrol/src/TimeVaryingObject.cpp
index 67b0df5c343..5e426eb31a6 100644
--- a/src/optimalcontrol/src/TimeVaryingObject.cpp
+++ b/src/optimalcontrol/src/TimeVaryingObject.cpp
@@ -19,59 +19,33 @@
 namespace iDynTree {
     namespace optimalcontrol {
 
-        TimeInvariantVector::TimeInvariantVector()
+        template<typename Object>
+        TimeInvariantObject<Object>::TimeInvariantObject()
         { }
 
-        TimeInvariantVector::TimeInvariantVector(const VectorDynSize &timeInvariantVector)
-        : m_timeInvariantVector(timeInvariantVector)
+        template<typename Object>
+        TimeInvariantObject<Object>::TimeInvariantObject(const Object &timeInvariantObject)
+            : m_invariantObject(timeInvariantObject)
         { }
 
-        VectorDynSize &TimeInvariantVector::get()
+        template<typename Object>
+        Object &TimeInvariantObject<Object>::get()
         {
-            return m_timeInvariantVector;
+            return m_invariantObject;
         }
 
-        const VectorDynSize &TimeInvariantVector::getObject(double /*time*/, bool &isValid)
+        template<typename Object>
+        const Object &TimeInvariantObject<Object>::get(double /*time*/, bool &isValid)
         {
             isValid = true;
-            return m_timeInvariantVector;
+            return m_invariantObject;
         }
 
-        TimeInvariantMatrix::TimeInvariantMatrix()
-        { }
-
-        TimeInvariantMatrix::TimeInvariantMatrix(const MatrixDynSize &timeInvariantMatrix)
-        : m_timeInvariantMatrix(timeInvariantMatrix)
-        { }
-
-        MatrixDynSize &TimeInvariantMatrix::get()
-        {
-            return m_timeInvariantMatrix;
-        }
+        template class TimeInvariantObject<double>;
 
-        const MatrixDynSize &TimeInvariantMatrix::getObject(double /*time*/, bool &isValid)
-        {
-            isValid = true;
-            return m_timeInvariantMatrix;
-        }
+        template class TimeInvariantObject<VectorDynSize>;
 
-        TimeInvariantDouble::TimeInvariantDouble()
-        { }
-
-        TimeInvariantDouble::TimeInvariantDouble(double timeInvariantDouble)
-            : m_timeInvariantDouble(timeInvariantDouble)
-        { }
-
-        double &TimeInvariantDouble::get()
-        {
-            return m_timeInvariantDouble;
-        }
-
-        const double &TimeInvariantDouble::getObject(double /*time*/, bool &isValid)
-        {
-            isValid = true;
-            return m_timeInvariantDouble;
-        }
+        template class TimeInvariantObject<MatrixDynSize>;
 
     }
 }

From 2bb3c2945872b20db4208072f328c5e2b32233fb Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 16:45:21 +0200
Subject: [PATCH 015/194] Moving from override to final for evaluation method
 of linear objects.

---
 src/optimalcontrol/include/iDynTree/LinearConstraint.h | 6 +++---
 src/optimalcontrol/include/iDynTree/LinearSystem.h     | 6 +++---
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
index b33bfee8846..7f7ed9070b5 100644
--- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h
+++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
@@ -51,17 +51,17 @@ namespace iDynTree {
             virtual bool evaluateConstraint(double time,
                                             const VectorDynSize& state,
                                             const VectorDynSize& control,
-                                            VectorDynSize& constraint) override;// lu <= [Ax, Au][x;u] <= lU
+                                            VectorDynSize& constraint) final;// lu <= [Ax, Au][x;u] <= lU
 
             virtual bool constraintJacobianWRTState(double time,
                                                     const VectorDynSize& state,
                                                     const VectorDynSize& control,
-                                                    MatrixDynSize& jacobian) override;
+                                                    MatrixDynSize& jacobian) final;
 
             virtual bool constraintJacobianWRTControl(double time,
                                                       const VectorDynSize& state,
                                                       const VectorDynSize& control,
-                                                      MatrixDynSize& jacobian) override;
+                                                      MatrixDynSize& jacobian) final;
 
         private:
             bool m_constrainsState, m_constrainsControl;
diff --git a/src/optimalcontrol/include/iDynTree/LinearSystem.h b/src/optimalcontrol/include/iDynTree/LinearSystem.h
index 43844df3895..a58963718d8 100644
--- a/src/optimalcontrol/include/iDynTree/LinearSystem.h
+++ b/src/optimalcontrol/include/iDynTree/LinearSystem.h
@@ -55,15 +55,15 @@ namespace iDynTree {
 
             virtual bool dynamics(const VectorDynSize& state,
                                   double time,
-                                  VectorDynSize& stateDynamics) override;
+                                  VectorDynSize& stateDynamics) final;
 
             virtual bool dynamicsStateFirstDerivative(const VectorDynSize& state,
                                                       double time,
-                                                      MatrixDynSize& dynamicsDerivative) override;
+                                                      MatrixDynSize& dynamicsDerivative) final;
 
             virtual bool dynamicsControlFirstDerivative(const VectorDynSize& state,
                                                         double time,
-                                                        MatrixDynSize& dynamicsDerivative) override;
+                                                        MatrixDynSize& dynamicsDerivative) final;
 
 
         private:

From 4ef02dbb27d696c40f43aed07c576542f5e755f2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 16:46:32 +0200
Subject: [PATCH 016/194] Added class QuadraticLikeCost.

In this way we can have a common ancestro between a generic quadratic cost, a L2Norm cost and a linear cost without the need of hiding QuadraticCost specialization methods.
---
 src/optimalcontrol/CMakeLists.txt             |   2 +
 .../include/iDynTree/L2NormCost.h             |   8 +-
 .../include/iDynTree/QuadraticCost.h          |  44 +--
 .../include/iDynTree/QuadraticLikeCost.h      |  84 ++++
 src/optimalcontrol/src/L2NormCost.cpp         |  46 +--
 src/optimalcontrol/src/QuadraticCost.cpp      | 342 +---------------
 src/optimalcontrol/src/QuadraticLikeCost.cpp  | 366 ++++++++++++++++++
 7 files changed, 485 insertions(+), 407 deletions(-)
 create mode 100644 src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
 create mode 100644 src/optimalcontrol/src/QuadraticLikeCost.cpp

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 1eec85c7022..3b5a258cce0 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -18,6 +18,7 @@ set(PUBLIC_HEADERS include/iDynTree/OptimalControl.h
                    include/iDynTree/SystemLineariser.h
                    include/iDynTree/Cost.h
                    include/iDynTree/Constraint.h
+                   include/iDynTree/QuadraticLikeCost.h
                    include/iDynTree/QuadraticCost.h
                    include/iDynTree/L2NormCost.h
                    include/iDynTree/LinearConstraint.h
@@ -48,6 +49,7 @@ set(SOURCES src/DynamicalSystem.cpp
             src/Cost.cpp
             src/Constraint.cpp
             src/QuadraticCost.cpp
+            src/QuadraticLikeCost.cpp
             src/L2NormCost.cpp
             src/LinearConstraint.cpp
             src/Controller.cpp
diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index ad8cc4e5e15..b53806bfd55 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -18,7 +18,7 @@
 #ifndef IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
 #define IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
 
-#include <iDynTree/QuadraticCost.h>
+#include <iDynTree/QuadraticLikeCost.h>
 #include <iDynTree/TimeVaryingObject.h>
 #include <memory>
 #include <string>
@@ -35,14 +35,10 @@ namespace iDynTree {
          * \ingroup iDynTreeExperimental
          */
 
-        class L2NormCost : public QuadraticCost {
+        class L2NormCost : public QuadraticLikeCost {
             class L2NormCostImplementation;
             L2NormCostImplementation *m_pimpl;
 
-            using QuadraticCost::setStateCost;  //avoid the user to access these methods to avoid confusion
-            using QuadraticCost::setControlCost;
-            using QuadraticCost::setCostBias;
-
         public:
 
             L2NormCost(const std::string& name, unsigned int stateDimension, unsigned int controlDimension); //assume identity as selector. Set the dimension to 0 to avoid weighting either the state or the cost
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index ea151c8fd05..310e7e44929 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -17,7 +17,7 @@
 #ifndef IDYNTREE_OPTIMALCONTROL_QUADRATICCOST_H
 #define IDYNTREE_OPTIMALCONTROL_QUADRATICCOST_H
 
-#include <iDynTree/Cost.h>
+#include <iDynTree/QuadraticLikeCost.h>
 #include <iDynTree/TimeVaryingObject.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
@@ -32,7 +32,7 @@ namespace iDynTree {
         */
 
        class QuadraticCost
-       : public Cost {
+       : public QuadraticLikeCost {
 
        public:
            QuadraticCost(const std::string& costName);
@@ -53,46 +53,6 @@ namespace iDynTree {
 
            bool setCostBias(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingStateCostBias,
                             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingControlCostBias);
-
-           virtual bool costEvaluation(double time,
-                                       const iDynTree::VectorDynSize& state,
-                                       const iDynTree::VectorDynSize& control,
-                                       double& costValue) override;//0.5 xT H x + gx + c
-
-           virtual bool costFirstPartialDerivativeWRTState(double time,
-                                                           const iDynTree::VectorDynSize& state,
-                                                           const iDynTree::VectorDynSize& control,
-                                                           iDynTree::VectorDynSize& partialDerivative) override;
-
-           virtual bool costFirstPartialDerivativeWRTControl(double time,
-                                                             const iDynTree::VectorDynSize& state,
-                                                             const iDynTree::VectorDynSize& control,
-                                                             iDynTree::VectorDynSize& partialDerivative) override;
-
-           virtual bool costSecondPartialDerivativeWRTState(double time,
-                                                            const iDynTree::VectorDynSize& state,
-                                                            const iDynTree::VectorDynSize& control,
-                                                            iDynTree::MatrixDynSize& partialDerivative) override;
-
-           virtual bool costSecondPartialDerivativeWRTControl(double time,
-                                                              const iDynTree::VectorDynSize& state,
-                                                              const iDynTree::VectorDynSize& control,
-                                                              iDynTree::MatrixDynSize& partialDerivative) override;
-
-
-           virtual bool costSecondPartialDerivativeWRTStateControl(double time,
-                                                                   const iDynTree::VectorDynSize& state,
-                                                                   const iDynTree::VectorDynSize& control,
-                                                                   iDynTree::MatrixDynSize& partialDerivative) override;
-
-       private:
-           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingStateHessian;
-           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingStateGradient;
-           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingStateCostBias;
-           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
-           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
-           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingControlCostBias;
-           double m_stateCostScale, m_controlCostScale;
        };
     }
 }
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
new file mode 100644
index 00000000000..898b4098481
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#ifndef IDYNTREE_OPTIMALCONTROL_QUADRATILIKECCOST_H
+#define IDYNTREE_OPTIMALCONTROL_QUADRATILIKECCOST_H
+
+#include <iDynTree/Cost.h>
+#include <iDynTree/TimeVaryingObject.h>
+#include <memory>
+
+namespace iDynTree {
+    namespace optimalcontrol {
+
+       /**
+        * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+        * \ingroup iDynTreeExperimental
+        */
+
+       class QuadraticLikeCost
+       : public Cost {
+
+       public:
+
+           virtual ~QuadraticLikeCost() override;
+
+           virtual bool costEvaluation(double time,
+                                       const iDynTree::VectorDynSize& state,
+                                       const iDynTree::VectorDynSize& control,
+                                       double& costValue) final;//0.5 xT H x + gx + c
+
+           virtual bool costFirstPartialDerivativeWRTState(double time,
+                                                           const iDynTree::VectorDynSize& state,
+                                                           const iDynTree::VectorDynSize& control,
+                                                           iDynTree::VectorDynSize& partialDerivative) final;
+
+           virtual bool costFirstPartialDerivativeWRTControl(double time,
+                                                             const iDynTree::VectorDynSize& state,
+                                                             const iDynTree::VectorDynSize& control,
+                                                             iDynTree::VectorDynSize& partialDerivative) final;
+
+           virtual bool costSecondPartialDerivativeWRTState(double time,
+                                                            const iDynTree::VectorDynSize& state,
+                                                            const iDynTree::VectorDynSize& control,
+                                                            iDynTree::MatrixDynSize& partialDerivative) final;
+
+           virtual bool costSecondPartialDerivativeWRTControl(double time,
+                                                              const iDynTree::VectorDynSize& state,
+                                                              const iDynTree::VectorDynSize& control,
+                                                              iDynTree::MatrixDynSize& partialDerivative) final;
+
+
+           virtual bool costSecondPartialDerivativeWRTStateControl(double time,
+                                                                   const iDynTree::VectorDynSize& state,
+                                                                   const iDynTree::VectorDynSize& control,
+                                                                   iDynTree::MatrixDynSize& partialDerivative) final;
+
+       protected:
+           QuadraticLikeCost(const std::string& costName);  //this class serves only as base class
+
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingStateHessian;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingStateGradient;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingStateCostBias;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
+           std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingControlCostBias;
+           bool m_costsState, m_costsControl;
+       };
+    }
+}
+
+#endif // IDYNTREE_OPTIMALCONTROL_QUADRATILIKECCOST_H
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 5e951422e6c..c90691da8b2 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -18,7 +18,6 @@
 #include <iDynTree/Core/MatrixDynSize.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/Utils.h>
-#include <iDynTree/Core/TestUtils.h>
 #include <cassert>
 
 #include <Eigen/Dense>
@@ -209,7 +208,7 @@ namespace iDynTree {
 
 
         L2NormCost::L2NormCost(const std::string &name, unsigned int stateDimension, unsigned int controlDimension)
-        : QuadraticCost(name)
+        : QuadraticLikeCost(name)
         , m_pimpl(new L2NormCostImplementation)
         {
             assert(m_pimpl);
@@ -217,34 +216,34 @@ namespace iDynTree {
             m_pimpl->initialize(stateDimension, controlDimension);
 
             if (m_pimpl->stateGradient) {
-                bool ok;
-                ok = setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
-                ASSERT_IS_TRUE(ok);
+                m_timeVaryingStateHessian = m_pimpl->stateHessian;
+                m_timeVaryingStateGradient = m_pimpl->stateGradient;
+                m_costsState = true;
             }
 
             if (m_pimpl->controlGradient) {
-                bool ok;
-                ok = setControlCost(m_pimpl->controlHessian, m_pimpl->controlGradient);
-                ASSERT_IS_TRUE(ok);
+                m_timeVaryingControlHessian = m_pimpl->controlHessian;
+                m_timeVaryingControlGradient = m_pimpl->controlGradient;
+                m_costsControl = true;
             }
         }
 
         L2NormCost::L2NormCost(const std::string &name, const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector)
-        : QuadraticCost(name)
+        : QuadraticLikeCost(name)
         , m_pimpl(new L2NormCostImplementation)
         {
             m_pimpl->initialize(stateSelector, controlSelector);
 
             if (m_pimpl->stateGradient) {
-                bool ok;
-                ok = setStateCost(m_pimpl->stateHessian, m_pimpl->stateGradient);
-                ASSERT_IS_TRUE(ok);
+                m_timeVaryingStateHessian = m_pimpl->stateHessian;
+                m_timeVaryingStateGradient = m_pimpl->stateGradient;
+                m_costsState = true;
             }
 
             if (m_pimpl->controlGradient) {
-                bool ok;
-                ok = setControlCost(m_pimpl->controlHessian, m_pimpl->controlGradient);
-                ASSERT_IS_TRUE(ok);
+                m_timeVaryingControlHessian = m_pimpl->controlHessian;
+                m_timeVaryingControlGradient = m_pimpl->controlGradient;
+                m_costsControl = true;
             }
         }
 
@@ -260,19 +259,20 @@ namespace iDynTree {
         {
             m_pimpl->addConstantPart = addItToTheCost;
             if (m_pimpl->addConstantPart) {
-                std::shared_ptr<TimeVaryingDouble> stateBias(new TimeInvariantDouble(0.0)), controlBias(new TimeInvariantDouble(0.0));
                 if (m_pimpl->stateGradient) {
-                    stateBias = m_pimpl->stateCostBias;
+                    m_timeVaryingStateCostBias = m_pimpl->stateCostBias;
                 }
-
                 if (m_pimpl->controlGradient) {
-                    controlBias = m_pimpl->controlCostBias;
+                    m_timeVaryingControlCostBias = m_pimpl->controlCostBias;
                 }
-
-                setCostBias(stateBias, controlBias);
-
             } else {
-                setCostBias(0.0, 0.0);
+                std::shared_ptr<TimeVaryingDouble> stateBias(new TimeInvariantDouble(0.0)), controlBias(new TimeInvariantDouble(0.0));
+                if (m_pimpl->stateGradient) {
+                    m_timeVaryingStateCostBias = stateBias;
+                }
+                if (m_pimpl->controlGradient) {
+                    m_timeVaryingControlCostBias = controlBias;
+                }
             }
         }
 
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index 17d943daf4e..71618c9e4b1 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -24,17 +24,8 @@ namespace iDynTree {
     namespace optimalcontrol {
 
         QuadraticCost::QuadraticCost(const std::string &costName)
-            : Cost(costName)
-            , m_timeVaryingStateHessian(nullptr)
-            , m_timeVaryingStateGradient(nullptr)
-            , m_timeVaryingControlHessian(nullptr)
-            , m_timeVaryingControlGradient(nullptr)
-            , m_stateCostScale(0.0)
-            , m_controlCostScale(0.0)
-        {
-            m_timeVaryingStateCostBias = std::make_shared<TimeInvariantDouble>(0.0);
-            m_timeVaryingControlCostBias = std::make_shared<TimeInvariantDouble>(0.0);
-        }
+            : QuadraticLikeCost(costName)
+        { }
 
         QuadraticCost::~QuadraticCost()
         { }
@@ -53,7 +44,7 @@ namespace iDynTree {
 
             m_timeVaryingStateHessian.reset(new TimeInvariantMatrix(stateHessian));
             m_timeVaryingStateGradient.reset(new TimeInvariantVector(stateGradient));
-            m_stateCostScale = 1.0;
+            m_costsState = true;
 
             return true;
         }
@@ -72,6 +63,7 @@ namespace iDynTree {
 
             m_timeVaryingStateHessian = timeVaryingStateHessian;
             m_timeVaryingStateGradient = timeVaryingStateGradient;
+            m_costsState = true;
 
             return true;
         }
@@ -90,7 +82,7 @@ namespace iDynTree {
 
             m_timeVaryingControlHessian.reset(new TimeInvariantMatrix(controlHessian));
             m_timeVaryingControlGradient.reset(new TimeInvariantVector(controlGradient));
-            m_controlCostScale = 1.0;
+            m_costsControl = true;
 
             return true;
         }
@@ -109,6 +101,7 @@ namespace iDynTree {
 
             m_timeVaryingControlHessian = timeVaryingControlHessian;
             m_timeVaryingControlGradient = timeVaryingControlGradient;
+            m_costsControl = true;
 
             return true;
         }
@@ -138,328 +131,5 @@ namespace iDynTree {
             return true;
         }
 
-        bool QuadraticCost::costEvaluation(double time,
-                                           const iDynTree::VectorDynSize& state,
-                                           const iDynTree::VectorDynSize& control,
-                                           double& costValue)
-        {
-            double stateCost = 0, controlCost = 0;
-
-            if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
-                bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != hessian.cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The state hessian at time: " << time << " is not squared.";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != state.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid state gradient at time: " << time << ".";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != gradient.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                isValid = false;
-                double stateBias = m_timeVaryingStateCostBias->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid state cost bias at time: " << time << ".";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                stateCost = m_stateCostScale * 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0) + toEigen(gradient).transpose()*toEigen(state) + stateBias;
-            }
-
-            if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
-                bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != hessian.cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The control hessian at time: " << time << " is not squared.";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != control.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid control gradient at time: " << time << ".";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != gradient.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                isValid = false;
-                double controlBias = m_timeVaryingControlCostBias->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid control cost bias at time: " << time << ".";
-                    reportError("QuadraticCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                controlCost = m_controlCostScale * 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0) + toEigen(gradient).transpose()*toEigen(control) + controlBias;
-            }
-
-            costValue = stateCost + controlCost;
-            return true;
-        }
-
-        bool QuadraticCost::costFirstPartialDerivativeWRTState(double time,
-                                                               const iDynTree::VectorDynSize& state,
-                                                               const iDynTree::VectorDynSize& /*control*/,
-                                                               iDynTree::VectorDynSize& partialDerivative)
-        {
-            partialDerivative.resize(state.size());
-
-            if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
-                bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != hessian.cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The state hessian at time: " << time << " is not squared.";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != state.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid state gradient at time: " << time << ".";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != gradient.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                toEigen(partialDerivative) = m_stateCostScale * toEigen(hessian) * toEigen(state) + toEigen(gradient);
-            } else {
-                partialDerivative.zero();
-            }
-
-            return true;
-        }
-
-        bool QuadraticCost::costFirstPartialDerivativeWRTControl(double time,
-                                                                 const iDynTree::VectorDynSize& /*state*/,
-                                                                 const iDynTree::VectorDynSize& control,
-                                                                 iDynTree::VectorDynSize& partialDerivative)
-        {
-            partialDerivative.resize(control.size());
-
-            if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
-                bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != hessian.cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The control hessian at time: " << time << " is not squared.";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != control.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                isValid = false;
-                const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid control gradient at time: " << time << ".";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != gradient.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
-                    reportError("QuadraticCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                toEigen(partialDerivative) = m_controlCostScale * toEigen(hessian) * toEigen(control) + toEigen(gradient);
-            } else {
-                partialDerivative.zero();
-            }
-            return true;
-        }
-
-        bool QuadraticCost::costSecondPartialDerivativeWRTState(double time,
-                                                                const iDynTree::VectorDynSize& state,
-                                                                const iDynTree::VectorDynSize& /*control*/,
-                                                                iDynTree::MatrixDynSize& partialDerivative)
-        {
-            partialDerivative.resize(state.size(), state.size());
-            if (!checkDoublesAreEqual(m_stateCostScale, 0, 1E-30)){
-                bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != hessian.cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The state hessian at time: " << time << " is not squared.";
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != state.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
-                    return false;
-                }
-
-                toEigen(partialDerivative) = m_stateCostScale * toEigen(hessian);
-            } else {
-                partialDerivative.zero();
-            }
-            return true;
-        }
-
-        bool QuadraticCost::costSecondPartialDerivativeWRTControl(double time,
-                                                                  const iDynTree::VectorDynSize& /*state*/,
-                                                                  const iDynTree::VectorDynSize& control,
-                                                                  iDynTree::MatrixDynSize& partialDerivative)
-        {
-            partialDerivative.resize(control.size(), control.size());
-
-            if (!checkDoublesAreEqual(m_controlCostScale, 0, 1E-30)){
-                bool isValid = false;
-                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
-
-                if (!isValid) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != hessian.cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The control hessian at time: " << time << " is not squared.";
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                if (hessian.rows() != control.size()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
-                    reportError("QuadraticCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
-                    return false;
-                }
-
-                toEigen(partialDerivative) = m_controlCostScale * toEigen(hessian);
-            } else {
-                partialDerivative.zero();
-            }
-            return true;
-        }
-
-
-        bool QuadraticCost::costSecondPartialDerivativeWRTStateControl(double /*time*/,
-                                                                const iDynTree::VectorDynSize& state,
-                                                                const iDynTree::VectorDynSize& control,
-                                                                iDynTree::MatrixDynSize& partialDerivative)
-        {
-            partialDerivative.resize(state.size(), control.size());
-            partialDerivative.zero();
-            return true;
-        }
-
-
     }
 }
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
new file mode 100644
index 00000000000..6591cd560ab
--- /dev/null
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -0,0 +1,366 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/QuadraticLikeCost.h>
+#include <iDynTree/Core/Utils.h>
+
+#include <iDynTree/Core/EigenHelpers.h>
+#include <cmath>
+
+namespace iDynTree {
+    namespace optimalcontrol {
+
+        QuadraticLikeCost::QuadraticLikeCost(const std::string &costName)
+            : Cost(costName)
+            , m_timeVaryingStateHessian(nullptr)
+            , m_timeVaryingStateGradient(nullptr)
+            , m_timeVaryingControlHessian(nullptr)
+            , m_timeVaryingControlGradient(nullptr)
+            , m_costsState(false)
+            , m_costsControl(false)
+        {
+            m_timeVaryingStateCostBias = std::make_shared<TimeInvariantDouble>(0.0);
+            m_timeVaryingControlCostBias = std::make_shared<TimeInvariantDouble>(0.0);
+        }
+
+        QuadraticLikeCost::~QuadraticLikeCost()
+        { }
+
+        bool QuadraticLikeCost::costEvaluation(double time,
+                                           const iDynTree::VectorDynSize& state,
+                                           const iDynTree::VectorDynSize& control,
+                                           double& costValue)
+        {
+            double stateCost = 0, controlCost = 0;
+
+            if (m_costsState){
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state gradient at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                double stateBias = m_timeVaryingStateCostBias->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state cost bias at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                stateCost = 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0) + toEigen(gradient).transpose()*toEigen(state) + stateBias;
+            }
+
+            if (m_costsControl){
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control gradient at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                double controlBias = m_timeVaryingControlCostBias->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control cost bias at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                controlCost = 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0) + toEigen(gradient).transpose()*toEigen(control) + controlBias;
+            }
+
+            costValue = stateCost + controlCost;
+            return true;
+        }
+
+        bool QuadraticLikeCost::costFirstPartialDerivativeWRTState(double time,
+                                                               const iDynTree::VectorDynSize& state,
+                                                               const iDynTree::VectorDynSize& /*control*/,
+                                                               iDynTree::VectorDynSize& partialDerivative)
+        {
+            partialDerivative.resize(state.size());
+
+            if (m_costsState){
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state gradient at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                toEigen(partialDerivative) = toEigen(hessian) * toEigen(state) + toEigen(gradient);
+            } else {
+                partialDerivative.zero();
+            }
+
+            return true;
+        }
+
+        bool QuadraticLikeCost::costFirstPartialDerivativeWRTControl(double time,
+                                                                 const iDynTree::VectorDynSize& /*state*/,
+                                                                 const iDynTree::VectorDynSize& control,
+                                                                 iDynTree::VectorDynSize& partialDerivative)
+        {
+            partialDerivative.resize(control.size());
+
+            if (m_costsControl){
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                isValid = false;
+                const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control gradient at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != gradient.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                toEigen(partialDerivative) = toEigen(hessian) * toEigen(control) + toEigen(gradient);
+            } else {
+                partialDerivative.zero();
+            }
+            return true;
+        }
+
+        bool QuadraticLikeCost::costSecondPartialDerivativeWRTState(double time,
+                                                                const iDynTree::VectorDynSize& state,
+                                                                const iDynTree::VectorDynSize& /*control*/,
+                                                                iDynTree::MatrixDynSize& partialDerivative)
+        {
+            partialDerivative.resize(state.size(), state.size());
+            if (m_costsState){
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state hessian at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the state dimension.";
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                partialDerivative = hessian;
+            } else {
+                partialDerivative.zero();
+            }
+            return true;
+        }
+
+        bool QuadraticLikeCost::costSecondPartialDerivativeWRTControl(double time,
+                                                                  const iDynTree::VectorDynSize& /*state*/,
+                                                                  const iDynTree::VectorDynSize& control,
+                                                                  iDynTree::MatrixDynSize& partialDerivative)
+        {
+            partialDerivative.resize(control.size(), control.size());
+
+            if (m_costsControl){
+                bool isValid = false;
+                const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control hessian at time: " << time << ".";
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != hessian.cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control hessian at time: " << time << " is not squared.";
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The hessian at time: " << time << " does not match the control dimension.";
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                partialDerivative = hessian;
+            } else {
+                partialDerivative.zero();
+            }
+            return true;
+        }
+
+
+        bool QuadraticLikeCost::costSecondPartialDerivativeWRTStateControl(double /*time*/,
+                                                                const iDynTree::VectorDynSize& state,
+                                                                const iDynTree::VectorDynSize& control,
+                                                                iDynTree::MatrixDynSize& partialDerivative)
+        {
+            partialDerivative.resize(state.size(), control.size());
+            partialDerivative.zero();
+            return true;
+        }
+
+
+    }
+}

From 919e6d52222af0439308293dd5177816ed0a0866 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 2 Jun 2018 17:03:50 +0200
Subject: [PATCH 017/194] Added check for pointers in QuadraticLikeCost.

---
 .../include/iDynTree/QuadraticLikeCost.h      |  6 +-
 src/optimalcontrol/src/QuadraticLikeCost.cpp  | 55 +++++++++++++++++++
 2 files changed, 58 insertions(+), 3 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
index 898b4098481..169b7a16a52 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
@@ -14,8 +14,8 @@
  * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
  */
 
-#ifndef IDYNTREE_OPTIMALCONTROL_QUADRATILIKECCOST_H
-#define IDYNTREE_OPTIMALCONTROL_QUADRATILIKECCOST_H
+#ifndef IDYNTREE_OPTIMALCONTROL_QUADRATICLIKECOST_H
+#define IDYNTREE_OPTIMALCONTROL_QUADRATICLIKECOST_H
 
 #include <iDynTree/Cost.h>
 #include <iDynTree/TimeVaryingObject.h>
@@ -81,4 +81,4 @@ namespace iDynTree {
     }
 }
 
-#endif // IDYNTREE_OPTIMALCONTROL_QUADRATILIKECCOST_H
+#endif // IDYNTREE_OPTIMALCONTROL_QUADRATICLIKECOST_H
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
index 6591cd560ab..c5ff832e1ad 100644
--- a/src/optimalcontrol/src/QuadraticLikeCost.cpp
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -48,6 +48,10 @@ namespace iDynTree {
 
             if (m_costsState){
                 bool isValid = false;
+                if (!m_timeVaryingStateHessian) {
+                    reportError("QuadraticLikeCost", "costEvaluation", "The state cost is activated but the timeVaryingStateHessian pointer is empty.");
+                    return false;
+                }
                 const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
                 if (!isValid) {
@@ -71,6 +75,11 @@ namespace iDynTree {
                     return false;
                 }
 
+                if (!m_timeVaryingStateGradient) {
+                    reportError("QuadraticLikeCost", "costEvaluation", "The state cost is activated but the timeVaryingStateGradient pointer is empty.");
+                    return false;
+                }
+
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
 
@@ -88,6 +97,10 @@ namespace iDynTree {
                     return false;
                 }
 
+                if (!m_timeVaryingStateCostBias) {
+                    reportError("QuadraticLikeCost", "costEvaluation", "The state cost is activated but the timeVaryingStateCostBias pointer is empty.");
+                    return false;
+                }
                 isValid = false;
                 double stateBias = m_timeVaryingStateCostBias->get(time, isValid);
 
@@ -102,6 +115,11 @@ namespace iDynTree {
             }
 
             if (m_costsControl){
+                if (!m_timeVaryingControlHessian) {
+                    reportError("QuadraticLikeCost", "costEvaluation", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
+                    return false;
+                }
+
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
@@ -126,6 +144,11 @@ namespace iDynTree {
                     return false;
                 }
 
+                if (!m_timeVaryingControlGradient) {
+                    reportError("QuadraticLikeCost", "costEvaluation", "The control cost is activated but the timeVaryingControlGradient pointer is empty.");
+                    return false;
+                }
+
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
 
@@ -143,6 +166,11 @@ namespace iDynTree {
                     return false;
                 }
 
+                if (!m_timeVaryingControlCostBias) {
+                    reportError("QuadraticLikeCost", "costEvaluation", "The control cost is activated but the timeVaryingControlCostBias pointer is empty.");
+                    return false;
+                }
+
                 isValid = false;
                 double controlBias = m_timeVaryingControlCostBias->get(time, isValid);
 
@@ -168,6 +196,11 @@ namespace iDynTree {
             partialDerivative.resize(state.size());
 
             if (m_costsState){
+                if (!m_timeVaryingStateHessian) {
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", "The state cost is activated but the timeVaryingStateHessian pointer is empty.");
+                    return false;
+                }
+
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
@@ -192,6 +225,11 @@ namespace iDynTree {
                     return false;
                 }
 
+                if (!m_timeVaryingStateGradient) {
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", "The state cost is activated but the timeVaryingStateGradient pointer is empty.");
+                    return false;
+                }
+
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
 
@@ -225,6 +263,11 @@ namespace iDynTree {
             partialDerivative.resize(control.size());
 
             if (m_costsControl){
+                if (!m_timeVaryingControlHessian) {
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
+                    return false;
+                }
+
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
@@ -249,6 +292,10 @@ namespace iDynTree {
                     return false;
                 }
 
+                if (!m_timeVaryingControlGradient) {
+                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlGradient pointer is empty.");
+                    return false;
+                }
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
 
@@ -280,6 +327,10 @@ namespace iDynTree {
         {
             partialDerivative.resize(state.size(), state.size());
             if (m_costsState){
+                if (!m_timeVaryingStateHessian) {
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTState", "The state cost is activated but the timeVaryingStateHessian pointer is empty.");
+                    return false;
+                }
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
@@ -319,6 +370,10 @@ namespace iDynTree {
             partialDerivative.resize(control.size(), control.size());
 
             if (m_costsControl){
+                if (!m_timeVaryingControlHessian) {
+                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
+                    return false;
+                }
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 

From b13d11e55e035bdddb2ec8cc75c264ee08105776 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 5 Jun 2018 18:31:36 +0200
Subject: [PATCH 018/194] Added linear cost.

The evaluation of QuadraticLike cost has changed. If a pointer is not present, it is considered as a zero.
---
 src/optimalcontrol/CMakeLists.txt             |   2 +
 .../include/iDynTree/LinearCost.h             |  58 ++++++++
 .../include/iDynTree/QuadraticCost.h          |   2 +-
 .../include/iDynTree/QuadraticLikeCost.h      |   1 -
 src/optimalcontrol/src/L2NormCost.cpp         |   4 -
 src/optimalcontrol/src/LinearCost.cpp         |  95 +++++++++++++
 src/optimalcontrol/src/QuadraticCost.cpp      |   4 -
 src/optimalcontrol/src/QuadraticLikeCost.cpp  | 133 ++++++++----------
 8 files changed, 213 insertions(+), 86 deletions(-)
 create mode 100644 src/optimalcontrol/include/iDynTree/LinearCost.h
 create mode 100644 src/optimalcontrol/src/LinearCost.cpp

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 3b5a258cce0..f21667e7dd0 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -22,6 +22,7 @@ set(PUBLIC_HEADERS include/iDynTree/OptimalControl.h
                    include/iDynTree/QuadraticCost.h
                    include/iDynTree/L2NormCost.h
                    include/iDynTree/LinearConstraint.h
+                   include/iDynTree/LinearCost.h
                    include/iDynTree/Controller.h
                    include/iDynTree/TimeRange.h
                    include/iDynTree/ConstraintsGroup.h
@@ -52,6 +53,7 @@ set(SOURCES src/DynamicalSystem.cpp
             src/QuadraticLikeCost.cpp
             src/L2NormCost.cpp
             src/LinearConstraint.cpp
+            src/LinearCost.cpp
             src/Controller.cpp
             src/FixedStepIntegrator.cpp
             src/RK4.cpp
diff --git a/src/optimalcontrol/include/iDynTree/LinearCost.h b/src/optimalcontrol/include/iDynTree/LinearCost.h
new file mode 100644
index 00000000000..0e69f30cf3a
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/LinearCost.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#ifndef IDYNTREE_OPTIMALCONTROL_LINEARCOST_H
+#define IDYNTREE_OPTIMALCONTROL_LINEARCOST_H
+
+#include <iDynTree/QuadraticLikeCost.h>
+#include <iDynTree/TimeVaryingObject.h>
+
+
+namespace iDynTree {
+    namespace optimalcontrol {
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+
+        class LinearCost : public QuadraticLikeCost {
+
+        public:
+            LinearCost(const std::string& costName);
+
+            ~LinearCost();
+
+            bool setStateCost(const iDynTree::VectorDynSize& stateGradient);
+
+            bool setStateCost(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> timeVaryingStateGradient);
+
+            bool setControlCost(const iDynTree::VectorDynSize& controlGradient);
+
+            bool setControlCost(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> timeVaryingControlGradient);
+
+            bool setCostBias(double stateCostBias, double controlCostBias);
+
+            bool setCostBias(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingStateCostBias,
+                             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingControlCostBias);
+
+        };
+
+
+    }
+}
+
+#endif // IDYNTREE_OPTIMALCONTROL_LINEARCOST_H
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index 310e7e44929..84253c1349a 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -37,7 +37,7 @@ namespace iDynTree {
        public:
            QuadraticCost(const std::string& costName);
 
-           virtual ~QuadraticCost() override;
+           ~QuadraticCost();
 
            bool setStateCost(const iDynTree::MatrixDynSize& stateHessian, const iDynTree::VectorDynSize& stateGradient);
 
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
index 169b7a16a52..05e6c5bbfbd 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
@@ -76,7 +76,6 @@ namespace iDynTree {
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingControlCostBias;
-           bool m_costsState, m_costsControl;
        };
     }
 }
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index c90691da8b2..ee31734c42b 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -218,13 +218,11 @@ namespace iDynTree {
             if (m_pimpl->stateGradient) {
                 m_timeVaryingStateHessian = m_pimpl->stateHessian;
                 m_timeVaryingStateGradient = m_pimpl->stateGradient;
-                m_costsState = true;
             }
 
             if (m_pimpl->controlGradient) {
                 m_timeVaryingControlHessian = m_pimpl->controlHessian;
                 m_timeVaryingControlGradient = m_pimpl->controlGradient;
-                m_costsControl = true;
             }
         }
 
@@ -237,13 +235,11 @@ namespace iDynTree {
             if (m_pimpl->stateGradient) {
                 m_timeVaryingStateHessian = m_pimpl->stateHessian;
                 m_timeVaryingStateGradient = m_pimpl->stateGradient;
-                m_costsState = true;
             }
 
             if (m_pimpl->controlGradient) {
                 m_timeVaryingControlHessian = m_pimpl->controlHessian;
                 m_timeVaryingControlGradient = m_pimpl->controlGradient;
-                m_costsControl = true;
             }
         }
 
diff --git a/src/optimalcontrol/src/LinearCost.cpp b/src/optimalcontrol/src/LinearCost.cpp
new file mode 100644
index 00000000000..64a7d97c4b2
--- /dev/null
+++ b/src/optimalcontrol/src/LinearCost.cpp
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/LinearCost.h>
+#include <iDynTree/Core/Utils.h>
+
+namespace iDynTree {
+    namespace optimalcontrol {
+
+        LinearCost::LinearCost(const std::string &costName)
+            : QuadraticLikeCost(costName)
+        { }
+
+        LinearCost::~LinearCost()
+        { }
+
+        bool LinearCost::setStateCost(const VectorDynSize &stateGradient)
+        {
+            m_timeVaryingStateGradient.reset(new TimeInvariantVector(stateGradient));
+
+            return true;
+        }
+
+        bool LinearCost::setStateCost(std::shared_ptr<TimeVaryingVector> timeVaryingStateGradient)
+        {
+            if (!timeVaryingStateGradient) {
+                reportError("LinearCost", "setStateCost", "Empty gradient pointer.");
+                return false;
+            }
+
+            m_timeVaryingStateGradient = timeVaryingStateGradient;
+
+            return true;
+        }
+
+        bool LinearCost::setControlCost(const VectorDynSize &controlGradient)
+        {
+
+            m_timeVaryingControlGradient.reset(new TimeInvariantVector(controlGradient));
+
+            return true;
+        }
+
+        bool LinearCost::setControlCost(std::shared_ptr<TimeVaryingVector> timeVaryingControlGradient)
+        {
+
+            if (!timeVaryingControlGradient) {
+                reportError("QuadraticCost", "setControlCost", "Empty gradient pointer.");
+                return false;
+            }
+
+            m_timeVaryingControlGradient = timeVaryingControlGradient;
+
+            return true;
+        }
+
+        bool LinearCost::setCostBias(double stateCostBias, double controlCostBias)
+        {
+            m_timeVaryingStateCostBias.reset(new TimeInvariantDouble(stateCostBias));
+            m_timeVaryingControlCostBias.reset(new TimeInvariantDouble(controlCostBias));
+            return true;
+        }
+
+        bool LinearCost::setCostBias(std::shared_ptr<TimeVaryingDouble> timeVaryingStateCostBias, std::shared_ptr<TimeVaryingDouble> timeVaryingControlCostBias)
+        {
+            if (!timeVaryingStateCostBias) {
+                reportError("QuadraticCost", "addCostBias", "The timeVaryingStateCostBias pointer is empty.");
+                return false;
+            }
+
+            if (!timeVaryingControlCostBias) {
+                reportError("QuadraticCost", "addCostBias", "The timeVaryingControlCostBias pointer is empty.");
+                return false;
+            }
+
+            m_timeVaryingStateCostBias = timeVaryingStateCostBias;
+            m_timeVaryingControlCostBias = timeVaryingControlCostBias;
+            return true;
+        }
+
+    }
+}
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index 71618c9e4b1..6101986889b 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -44,7 +44,6 @@ namespace iDynTree {
 
             m_timeVaryingStateHessian.reset(new TimeInvariantMatrix(stateHessian));
             m_timeVaryingStateGradient.reset(new TimeInvariantVector(stateGradient));
-            m_costsState = true;
 
             return true;
         }
@@ -63,7 +62,6 @@ namespace iDynTree {
 
             m_timeVaryingStateHessian = timeVaryingStateHessian;
             m_timeVaryingStateGradient = timeVaryingStateGradient;
-            m_costsState = true;
 
             return true;
         }
@@ -82,7 +80,6 @@ namespace iDynTree {
 
             m_timeVaryingControlHessian.reset(new TimeInvariantMatrix(controlHessian));
             m_timeVaryingControlGradient.reset(new TimeInvariantVector(controlGradient));
-            m_costsControl = true;
 
             return true;
         }
@@ -101,7 +98,6 @@ namespace iDynTree {
 
             m_timeVaryingControlHessian = timeVaryingControlHessian;
             m_timeVaryingControlGradient = timeVaryingControlGradient;
-            m_costsControl = true;
 
             return true;
         }
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
index c5ff832e1ad..0dd4cd7ee3b 100644
--- a/src/optimalcontrol/src/QuadraticLikeCost.cpp
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -17,6 +17,7 @@
 #include <iDynTree/QuadraticLikeCost.h>
 #include <iDynTree/Core/Utils.h>
 
+#include <Eigen/Dense>
 #include <iDynTree/Core/EigenHelpers.h>
 #include <cmath>
 
@@ -29,8 +30,6 @@ namespace iDynTree {
             , m_timeVaryingStateGradient(nullptr)
             , m_timeVaryingControlHessian(nullptr)
             , m_timeVaryingControlGradient(nullptr)
-            , m_costsState(false)
-            , m_costsControl(false)
         {
             m_timeVaryingStateCostBias = std::make_shared<TimeInvariantDouble>(0.0);
             m_timeVaryingControlCostBias = std::make_shared<TimeInvariantDouble>(0.0);
@@ -46,12 +45,8 @@ namespace iDynTree {
         {
             double stateCost = 0, controlCost = 0;
 
-            if (m_costsState){
-                bool isValid = false;
-                if (!m_timeVaryingStateHessian) {
-                    reportError("QuadraticLikeCost", "costEvaluation", "The state cost is activated but the timeVaryingStateHessian pointer is empty.");
-                    return false;
-                }
+            bool isValid = false;
+            if (m_timeVaryingStateHessian) {
                 const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
                 if (!isValid) {
@@ -74,11 +69,10 @@ namespace iDynTree {
                     reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
                     return false;
                 }
+                stateCost += 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0);
+            }
 
-                if (!m_timeVaryingStateGradient) {
-                    reportError("QuadraticLikeCost", "costEvaluation", "The state cost is activated but the timeVaryingStateGradient pointer is empty.");
-                    return false;
-                }
+            if (m_timeVaryingStateGradient) {
 
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
@@ -90,17 +84,17 @@ namespace iDynTree {
                     return false;
                 }
 
-                if (hessian.rows() != gradient.size()) {
+                if (state.size() != gradient.size()) {
                     std::ostringstream errorMsg;
-                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
+                    errorMsg << "The gradient at time: " << time << " have dimensions different from the state.";
                     reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
                     return false;
                 }
+                stateCost += toEigen(gradient).transpose()*toEigen(state);
+            }
+
+            if (m_timeVaryingStateCostBias) {
 
-                if (!m_timeVaryingStateCostBias) {
-                    reportError("QuadraticLikeCost", "costEvaluation", "The state cost is activated but the timeVaryingStateCostBias pointer is empty.");
-                    return false;
-                }
                 isValid = false;
                 double stateBias = m_timeVaryingStateCostBias->get(time, isValid);
 
@@ -111,16 +105,12 @@ namespace iDynTree {
                     return false;
                 }
 
-                stateCost = 0.5 * (toEigen(state).transpose() * toEigen(hessian) * toEigen(state))(0) + toEigen(gradient).transpose()*toEigen(state) + stateBias;
+                stateCost += stateBias;
             }
 
-            if (m_costsControl){
-                if (!m_timeVaryingControlHessian) {
-                    reportError("QuadraticLikeCost", "costEvaluation", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
-                    return false;
-                }
+            if (m_timeVaryingControlHessian) {
 
-                bool isValid = false;
+                isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
                 if (!isValid) {
@@ -144,10 +134,10 @@ namespace iDynTree {
                     return false;
                 }
 
-                if (!m_timeVaryingControlGradient) {
-                    reportError("QuadraticLikeCost", "costEvaluation", "The control cost is activated but the timeVaryingControlGradient pointer is empty.");
-                    return false;
-                }
+                controlCost += 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0);
+            }
+
+            if (m_timeVaryingControlGradient) {
 
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
@@ -159,17 +149,16 @@ namespace iDynTree {
                     return false;
                 }
 
-                if (hessian.rows() != gradient.size()) {
+                if (control.size() != gradient.size()) {
                     std::ostringstream errorMsg;
-                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
+                    errorMsg << "The control hessian at time " << time << " have dimension different from the control dimension.";
                     reportError("QuadraticLikeCost", "costEvaluation", errorMsg.str().c_str());
                     return false;
                 }
+                controlCost += toEigen(gradient).transpose()*toEigen(control);
+            }
 
-                if (!m_timeVaryingControlCostBias) {
-                    reportError("QuadraticLikeCost", "costEvaluation", "The control cost is activated but the timeVaryingControlCostBias pointer is empty.");
-                    return false;
-                }
+            if (m_timeVaryingControlCostBias) {
 
                 isValid = false;
                 double controlBias = m_timeVaryingControlCostBias->get(time, isValid);
@@ -181,7 +170,7 @@ namespace iDynTree {
                     return false;
                 }
 
-                controlCost = 0.5 * (toEigen(control).transpose() * toEigen(hessian) * toEigen(control))(0) + toEigen(gradient).transpose()*toEigen(control) + controlBias;
+                controlCost += controlBias;
             }
 
             costValue = stateCost + controlCost;
@@ -194,14 +183,12 @@ namespace iDynTree {
                                                                iDynTree::VectorDynSize& partialDerivative)
         {
             partialDerivative.resize(state.size());
+            partialDerivative.zero();
 
-            if (m_costsState){
-                if (!m_timeVaryingStateHessian) {
-                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", "The state cost is activated but the timeVaryingStateHessian pointer is empty.");
-                    return false;
-                }
+            bool isValid = false;
+
+            if (m_timeVaryingStateHessian) {
 
-                bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
                 if (!isValid) {
@@ -224,11 +211,10 @@ namespace iDynTree {
                     reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
                     return false;
                 }
+                toEigen(partialDerivative) += toEigen(hessian) * toEigen(state);
+            }
 
-                if (!m_timeVaryingStateGradient) {
-                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", "The state cost is activated but the timeVaryingStateGradient pointer is empty.");
-                    return false;
-                }
+            if (m_timeVaryingStateGradient) {
 
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingStateGradient->get(time, isValid);
@@ -240,16 +226,14 @@ namespace iDynTree {
                     return false;
                 }
 
-                if (hessian.rows() != gradient.size()) {
+                if (state.size() != gradient.size()) {
                     std::ostringstream errorMsg;
-                    errorMsg << "The state hessian and the gradient at time: " << time << " have different dimensions.";
+                    errorMsg << "The state and the gradient at time: " << time << " have different dimensions.";
                     reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
                     return false;
                 }
 
-                toEigen(partialDerivative) = toEigen(hessian) * toEigen(state) + toEigen(gradient);
-            } else {
-                partialDerivative.zero();
+                toEigen(partialDerivative) += toEigen(gradient);
             }
 
             return true;
@@ -261,14 +245,11 @@ namespace iDynTree {
                                                                  iDynTree::VectorDynSize& partialDerivative)
         {
             partialDerivative.resize(control.size());
+            partialDerivative.zero();
+            bool isValid = false;
 
-            if (m_costsControl){
-                if (!m_timeVaryingControlHessian) {
-                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
-                    return false;
-                }
+            if (m_timeVaryingControlHessian) {
 
-                bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 
                 if (!isValid) {
@@ -291,11 +272,11 @@ namespace iDynTree {
                     reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
                     return false;
                 }
+                toEigen(partialDerivative) += toEigen(hessian) * toEigen(control);
+            }
+
+            if (m_timeVaryingControlGradient) {
 
-                if (!m_timeVaryingControlGradient) {
-                    reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlGradient pointer is empty.");
-                    return false;
-                }
                 isValid = false;
                 const VectorDynSize &gradient = m_timeVaryingControlGradient->get(time, isValid);
 
@@ -306,17 +287,17 @@ namespace iDynTree {
                     return false;
                 }
 
-                if (hessian.rows() != gradient.size()) {
+                if (control.size() != gradient.size()) {
                     std::ostringstream errorMsg;
-                    errorMsg << "The control hessian and the gradient at time: " << time << " have different dimensions.";
+                    errorMsg << "The control and the gradient at time: " << time << " have different dimensions.";
                     reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
                     return false;
                 }
 
-                toEigen(partialDerivative) = toEigen(hessian) * toEigen(control) + toEigen(gradient);
-            } else {
-                partialDerivative.zero();
+                toEigen(partialDerivative) += toEigen(gradient);
             }
+
+
             return true;
         }
 
@@ -326,11 +307,9 @@ namespace iDynTree {
                                                                 iDynTree::MatrixDynSize& partialDerivative)
         {
             partialDerivative.resize(state.size(), state.size());
-            if (m_costsState){
-                if (!m_timeVaryingStateHessian) {
-                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTState", "The state cost is activated but the timeVaryingStateHessian pointer is empty.");
-                    return false;
-                }
+
+            if (m_timeVaryingStateHessian) {
+
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingStateHessian->get(time, isValid);
 
@@ -356,8 +335,11 @@ namespace iDynTree {
                 }
 
                 partialDerivative = hessian;
+
             } else {
+
                 partialDerivative.zero();
+
             }
             return true;
         }
@@ -369,11 +351,10 @@ namespace iDynTree {
         {
             partialDerivative.resize(control.size(), control.size());
 
-            if (m_costsControl){
-                if (!m_timeVaryingControlHessian) {
-                    reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
-                    return false;
-                }
+            if (m_timeVaryingControlHessian) {
+                reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
+                return false;
+
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
 

From f6b15d9fab3f4f128d85be2e1d0a34a367cc507c Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Jun 2018 12:00:28 +0200
Subject: [PATCH 019/194] Modified ConstraintGroup to allow for
 LinearConstraints.

Modified the ConstraintGroupTest. Modified some random comments. Avoiding to initialize the QuadraticLikeCost bias. It will be automatically ignored.
---
 .../include/iDynTree/ConstraintsGroup.h       |  15 ++
 .../include/iDynTree/OptimizationProblem.h    |   4 +-
 src/optimalcontrol/src/ConstraintsGroup.cpp   | 136 ++++++++++--------
 src/optimalcontrol/src/QuadraticLikeCost.cpp  |   7 +-
 .../tests/ConstraintsGroupTest.cpp            |  17 ++-
 5 files changed, 112 insertions(+), 67 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index 26c6edc9af0..b6ea0729744 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -29,6 +29,7 @@ namespace iDynTree {
     namespace optimalcontrol {
 
         class Constraint;
+        class LinearConstraint;
         class TimeRange;
 
         /**
@@ -76,6 +77,14 @@ namespace iDynTree {
              */
             bool addConstraint(std::shared_ptr<Constraint> constraint, const TimeRange& timeRange);
 
+            /**
+             * @brief Add a linear constraint to the group
+             * @param[in] constraint Shared pointer to the user defined constraint.
+             * @param[in] timeRange Time range in which the constraint will be enabled.
+             * @return True if successfull. Posible causes of failures are: empty pointer, dimension bigger than maxConstraintSize, invalid TimeRange.
+             */
+            bool addConstraint(std::shared_ptr<LinearConstraint> linearConstraint, const TimeRange& timeRange);
+
             /**
              * @brief Update the TimeRange of a previously added constraint.
              * @param[in] name The name of the constraint whose TimeRange has to be updated.
@@ -199,6 +208,12 @@ namespace iDynTree {
              */
             const std::vector<std::string> listConstraints() const;
 
+            /**
+             * @brief Tells if the groups contains only linear constraints.
+             * @return true if contains only linear constraints. False if at least one generic constraint is added.
+             */
+            bool isLinearGroup() const;
+
         private:
             class ConstraintsGroupPimpl;
             ConstraintsGroupPimpl* m_pimpl;
diff --git a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
index 066a83c602a..8bea66bf77d 100644
--- a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
@@ -113,9 +113,9 @@ namespace iDynTree {
 
             virtual bool evaluateCostFunction(double& costValue);
 
-            virtual bool evaluateCostGradient(VectorDynSize& gradient);
+            virtual bool evaluateCostGradient(VectorDynSize& gradient); //for quadratic costs this corresponds to Hx + g !
 
-            virtual bool evaluateCostHessian(MatrixDynSize& hessian); //using dense matrices, but the sparsity pattern is still obtained
+            virtual bool evaluateCostHessian(MatrixDynSize& hessian); //using dense matrices, but the sparsity pattern is still obtained. Initialize hessian to zero in case of dense solvers
 
             virtual bool evaluateConstraints(VectorDynSize& constraints);
 
diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index f5f83c73203..3023a629152 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -19,6 +19,7 @@
 #include <iDynTree/Core/Utils.h>
 #include <iDynTree/ConstraintsGroup.h>
 #include <iDynTree/Constraint.h>
+#include <iDynTree/LinearConstraint.h>
 #include <iDynTree/TimeRange.h>
 #include <iDynTree/Core/EigenHelpers.h>
 #include <Eigen/Dense>
@@ -50,12 +51,76 @@ namespace optimalcontrol {
             std::string name;
             unsigned int maxConstraintSize;
             std::vector<TimeRange> timeRanges;
+            bool isLinearGroup = true;
 
             std::vector<TimedConstraint_ptr>::reverse_iterator findActiveConstraint(double time){
                 return std::find_if(orderedIntervals.rbegin(),
                                     orderedIntervals.rend(),
                                     [time](const TimedConstraint_ptr & a) -> bool { return a->timeRange.isInRange(time); }); //find the last element in the vector with init time lower than the specified time
             }
+
+            bool addConstraint(std::shared_ptr<Constraint> constraint, const TimeRange &timeRange)
+            {
+                if (!constraint){
+                    reportError("ConstraintsGroup", "addConstraint", "Empty constraint pointer.");
+                    return false;
+                }
+
+                if (constraint->constraintSize() > maxConstraintSize){
+                    reportError("ConstraintsGroup", "addConstraint", "The constraint dimension is greater than the maximum allowed by the group.");
+                    return false;
+                }
+
+                if (timeRange == TimeRange::AnyTime()){
+                    if (group.size() != 0){
+                        reportError("ConstraintsGroup", "addConstraint",
+                                    "Only one constraint is allowed in a group if the timeRange is AnyTime.");
+                        return false;
+                    }
+                } else {
+                    if (!timeRange.isValid()){
+                        reportError("ConstraintsGroup", "addConstraint", "Invalid timeRange.");
+                        return false;
+                    }
+                }
+
+                if(group.size() > 0){
+                    GroupOfConstraintsMap::iterator constraintIterator;
+                    constraintIterator = group.find(constraint->name());
+                    if(constraintIterator != group.end()){
+                        std::ostringstream errorMsg;
+                        errorMsg << "A constraint named " << constraint->name()
+                                 <<" already exists in the group "<< name << " .";
+                        reportError("ConstraintsGroup", "addConstraint", errorMsg.str().c_str());
+                        return false;
+                    }
+                }
+
+                //add constraints in the group
+                TimedConstraint_ptr newConstraint = std::make_shared<TimedConstraint>();
+
+                newConstraint->timeRange = timeRange;
+                newConstraint->constraint = constraint;
+                newConstraint->constraintBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()));
+                newConstraint->stateJacobianBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()), static_cast<unsigned int>(constraint->expectedStateSpaceSize()));
+                newConstraint->controlJacobianBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()), static_cast<unsigned int>(constraint->expectedControlSpaceSize()));
+
+                std::pair< GroupOfConstraintsMap::iterator, bool> result;
+                result = group.insert(GroupOfConstraintsMap::value_type(constraint->name(), newConstraint));
+
+                if(!result.second){
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to add constraint "<<constraint->name() << std::endl;
+                    reportError("ConstraintsGroup", "addConstraint", errorMsg.str().c_str());
+                    return false;
+                }
+
+                orderedIntervals.push_back(result.first->second); //register the time range in order to have the constraints ordered by init time. result.first->second is the TimedConstraint_ptr of the newly inserted TimedConstraint.
+                std::sort(orderedIntervals.begin(), orderedIntervals.end(), [](const TimedConstraint_ptr&a, const TimedConstraint_ptr&b) { return a->timeRange < b->timeRange;}); //reorder the vector
+
+                return true;
+            }
+
         };
 
         ConstraintsGroup::ConstraintsGroup(const std::string &name, unsigned int maxConstraintSize)
@@ -86,67 +151,13 @@ namespace optimalcontrol {
 
         bool ConstraintsGroup::addConstraint(std::shared_ptr<Constraint> constraint, const TimeRange &timeRange)
         {
-            if (!constraint){
-                reportError("ConstraintsGroup", "addConstraint", "Empty constraint pointer.");
-                return false;
-            }
-
-            if (constraint->constraintSize() > m_pimpl->maxConstraintSize){
-                reportError("ConstraintsGroup", "addConstraint", "The constraint dimension is greater than the maximum allowed by the group.");
-                return false;
-            }
-
-            if (timeRange == TimeRange::AnyTime()){
-                if (numberOfConstraints() != 0){
-                    reportError("ConstraintsGroup", "addConstraint",
-                                "Only one constraint is allowed in a group if the timeRange is AnyTime.");
-                    return false;
-                }
-            } else {
-                if (!timeRange.isValid()){
-                    reportError("ConstraintsGroup", "addConstraint", "Invalid timeRange.");
-                    return false;
-                }
-            }
-
-            if(m_pimpl->group.size() > 0){
-                GroupOfConstraintsMap::iterator constraintIterator;
-                constraintIterator = m_pimpl->group.find(constraint->name());
-                if(constraintIterator != m_pimpl->group.end()){
-                    std::ostringstream errorMsg;
-                    errorMsg << "A constraint named " << constraint->name()
-                             <<" already exists in the group "<< m_pimpl->name << " .";
-                    reportError("ConstraintsGroup", "addConstraint", errorMsg.str().c_str());
-                    return false;
-                }
-            }
-
-            //add constraints in the group
-            TimedConstraint_ptr newConstraint = std::make_shared<TimedConstraint>();
-
-            newConstraint->timeRange = timeRange;
-            newConstraint->constraint = constraint;
-            newConstraint->constraintBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()));
-            newConstraint->stateJacobianBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()), static_cast<unsigned int>(constraint->expectedStateSpaceSize()));
-            newConstraint->controlJacobianBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()), static_cast<unsigned int>(constraint->expectedControlSpaceSize()));
-
-            std::pair< GroupOfConstraintsMap::iterator, bool> result;
-            result = m_pimpl->group.insert(GroupOfConstraintsMap::value_type(constraint->name(), newConstraint));
-
-            if(!result.second){
-                std::ostringstream errorMsg;
-                errorMsg << "Unable to add constraint "<<constraint->name() << std::endl;
-                reportError("ConstraintsGroup", "addConstraint", errorMsg.str().c_str());
-                return false;
-            }
-
-            m_pimpl->orderedIntervals.push_back(result.first->second); //register the time range in order to have the constraints ordered by init time. result.first->second is the TimedConstraint_ptr of the newly inserted TimedConstraint.
-            std::sort(m_pimpl->orderedIntervals.begin(), m_pimpl->orderedIntervals.end(), [](const TimedConstraint_ptr&a, const TimedConstraint_ptr&b) { return a->timeRange < b->timeRange;}); //reorder the vector
-
-
-            //the jacobian should be updated
+            m_pimpl->isLinearGroup = false;
+            return m_pimpl->addConstraint(constraint, timeRange);
+        }
 
-            return true;
+        bool ConstraintsGroup::addConstraint(std::shared_ptr<LinearConstraint> linearConstraint, const TimeRange &timeRange)
+        {
+            return m_pimpl->addConstraint(linearConstraint, timeRange);
         }
 
         bool ConstraintsGroup::updateTimeRange(const std::string &name, const TimeRange &timeRange)
@@ -499,5 +510,10 @@ namespace optimalcontrol {
             return output;
         }
 
+        bool ConstraintsGroup::isLinearGroup() const
+        {
+            return m_pimpl->isLinearGroup;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
index 0dd4cd7ee3b..150291bf0ef 100644
--- a/src/optimalcontrol/src/QuadraticLikeCost.cpp
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -28,12 +28,11 @@ namespace iDynTree {
             : Cost(costName)
             , m_timeVaryingStateHessian(nullptr)
             , m_timeVaryingStateGradient(nullptr)
+            , m_timeVaryingStateCostBias(nullptr)
             , m_timeVaryingControlHessian(nullptr)
             , m_timeVaryingControlGradient(nullptr)
-        {
-            m_timeVaryingStateCostBias = std::make_shared<TimeInvariantDouble>(0.0);
-            m_timeVaryingControlCostBias = std::make_shared<TimeInvariantDouble>(0.0);
-        }
+            , m_timeVaryingControlCostBias(nullptr)
+        { }
 
         QuadraticLikeCost::~QuadraticLikeCost()
         { }
diff --git a/src/optimalcontrol/tests/ConstraintsGroupTest.cpp b/src/optimalcontrol/tests/ConstraintsGroupTest.cpp
index dd1ac68c09b..fee149b6c8f 100644
--- a/src/optimalcontrol/tests/ConstraintsGroupTest.cpp
+++ b/src/optimalcontrol/tests/ConstraintsGroupTest.cpp
@@ -14,6 +14,7 @@
  * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
  */
 #include <iDynTree/Constraint.h>
+#include <iDynTree/LinearConstraint.h>
 #include <iDynTree/ConstraintsGroup.h>
 #include <iDynTree/TimeRange.h>
 #include <iDynTree/Core/VectorDynSize.h>
@@ -40,22 +41,32 @@ class DummyConstraint1 : public Constraint{
 bool groupTest(){
     std::shared_ptr<DummyConstraint1> constraint1 = std::make_shared<DummyConstraint1>("Dummy1");
     std::shared_ptr<DummyConstraint1> constraint2 = std::make_shared<DummyConstraint1>("Dummy2");
-
+    std::shared_ptr<iDynTree::optimalcontrol::LinearConstraint> constraint3;
+    constraint3 = std::make_shared<iDynTree::optimalcontrol::LinearConstraint>(1, "LinConstraint");
+    iDynTree::MatrixDynSize constraintMatrix(1,1);
+    constraintMatrix(0, 0) = 1;
+    ASSERT_IS_TRUE(constraint3->setStateConstraintMatrix(constraintMatrix));
 
     iDynTree::VectorDynSize upperbound(1);
     upperbound.zero();
 
     ASSERT_IS_TRUE(constraint1->setUpperBound(upperbound));
     ASSERT_IS_TRUE(constraint2->setUpperBound(upperbound));
+    ASSERT_IS_TRUE(constraint3->setUpperBound(upperbound));
 
 
     ConstraintsGroup newGroup("dummyGroup", 2);
+    ConstraintsGroup linearGroup("linGroup", 2);
 
     TimeRange range;
     ASSERT_IS_TRUE(range.setTimeInterval(1.0, 2.0));
 
     ASSERT_IS_TRUE(newGroup.addConstraint(constraint1, range));
 
+    ASSERT_IS_TRUE(linearGroup.addConstraint(constraint3, range));
+
+    ASSERT_IS_TRUE(linearGroup.isLinearGroup());
+
     ASSERT_IS_TRUE(range.setTimeInterval(0.0, 1.0));
 
     ASSERT_IS_TRUE(newGroup.addConstraint(constraint2, range));
@@ -70,6 +81,10 @@ bool groupTest(){
     ASSERT_IS_TRUE(newGroup.evaluateConstraints(1.5, dummyState, dummyControl, result));
     ASSERT_EQUAL_VECTOR_TOL(result, expected, 1e-10);
 
+    ASSERT_IS_TRUE(linearGroup.isFeasibilePoint(1.5, dummyState, dummyControl));
+    ASSERT_IS_TRUE(linearGroup.evaluateConstraints(1.5, dummyState, dummyControl, result));
+    ASSERT_EQUAL_VECTOR_TOL(result, expected, 1e-10);
+
     dummyState(0) = -2.0;
     expected(0) = dummyState(0);
     expected(1) = 0.0;

From a9327ce6cc2d5c988f7a038eedfb1209c535e011 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Jun 2018 14:01:33 +0200
Subject: [PATCH 020/194] Considering linear costs, quadratic costs and linear
 constraints to the OC problem.

Tests have been modified too. Deprecated method with a typo.
---
 .../include/iDynTree/OptimalControlProblem.h  |  49 ++-
 .../src/OptimalControlProblem.cpp             | 296 +++++++++++++-----
 src/optimalcontrol/tests/IntegratorsTest.cpp  |  22 +-
 .../tests/MultipleShootingTest.cpp            |   2 +-
 src/optimalcontrol/tests/OCProblemTest.cpp    |   2 +-
 5 files changed, 278 insertions(+), 93 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index f4c683a015e..57a55cddf21 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -21,6 +21,8 @@
 #include <vector>
 #include <string>
 
+#include <iDynTree/Core/Utils.h>
+
 namespace iDynTree {
 
     class VectorDynSize;
@@ -30,8 +32,11 @@ namespace iDynTree {
 
         class DynamicalSystem;
         class Constraint;
+        class LinearConstraint;
         class ConstraintsGroup;
         class Cost;
+        class QuadraticLikeCost;
+        class LinearCost;
         class TimeRange;
 
         /**
@@ -59,29 +64,67 @@ namespace iDynTree {
 
             bool removeGroupOfConstraints(const std::string& name);
 
-            bool addContraint(std::shared_ptr<Constraint> newConstraint); // this apply for the full horizon. It creates a group named with the same name and containing only newConstraint
+            IDYNTREE_DEPRECATED_WITH_MSG("Use addConstraint() instead")
+            bool addContraint(std::shared_ptr<Constraint> newConstraint) {
+                return addConstraint(newConstraint);
+            }
+
+            bool addConstraint(std::shared_ptr<Constraint> newConstraint); // this apply for the full horizon. It creates a group named with the same name and containing only newConstraint
+
+            bool addConstraint(std::shared_ptr<LinearConstraint> newConstraint);
+
             bool removeConstraint(const std::string& name); //this removes only the constraints added with the above method
+
             unsigned int countConstraints() const;
+
+            unsigned int countLinearConstraints() const;
+
             unsigned int getConstraintsDimension() const;
+
             const std::vector<std::string> listConstraints() const;
+
             const std::vector<std::string> listGroups() const; //the i-th entry of the list contains the i-th constraint displayed with listConstraints()
 
             std::vector<TimeRange>& getConstraintsTimeRanges() const;
 
+            std::vector<size_t>& getLinearConstraintsIndeces() const;
+
             // Cost can be:
             // Mayer term
             // Lagrange term
             bool addMayerTerm(double weight, std::shared_ptr<Cost> cost); // final cost
 
+            bool addMayerTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost); // final cost
+
+            bool addMayerTerm(double weight, std::shared_ptr<LinearCost> linearCost); // final cost
+
             bool addLagrangeTerm(double weight, std::shared_ptr<Cost> cost); // integral cost
 
+            bool addLagrangeTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost); // integral cost
+
+            bool addLagrangeTerm(double weight, std::shared_ptr<LinearCost> linearCost); // integral cost
+
             bool addLagrangeTerm(double weight,
                                  double startingTime,
                                  double finalTime,
                                  std::shared_ptr<Cost> cost); // integral cost with explicit integration limits
 
+            bool addLagrangeTerm(double weight,
+                                 double startingTime,
+                                 double finalTime,
+                                 std::shared_ptr<QuadraticLikeCost> quadraticCost); // integral cost with explicit integration limits
+
+            bool addLagrangeTerm(double weight,
+                                 double startingTime,
+                                 double finalTime,
+                                 std::shared_ptr<LinearCost> linearCost); // integral cost with explicit integration limits
+
             bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<Cost> cost);
 
+            bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<QuadraticLikeCost> quadraticCost);
+
+            bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<LinearCost> linearCost);
+
             bool updateCostTimeRange(const std::string& name, double newStartingTime, double newEndTime);
 
             bool updateCostTimeRange(const std::string& name, const TimeRange& newTimeRange);
@@ -90,6 +133,10 @@ namespace iDynTree {
 
             std::vector<TimeRange>& getCostsTimeRanges() const;
 
+            bool hasOnlyLinearCosts() const;
+
+            bool hasOnlyQuadraticCosts() const;
+
             bool setStateLowerBound(const VectorDynSize& minState);
 
             bool setStateUpperBound(const VectorDynSize& maxState);
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 0fd819bf1f4..073de5c13da 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -18,7 +18,10 @@
 #include <iDynTree/DynamicalSystem.h>
 #include <iDynTree/ConstraintsGroup.h>
 #include <iDynTree/Constraint.h>
+#include <iDynTree/LinearConstraint.h>
 #include <iDynTree/Cost.h>
+#include <iDynTree/QuadraticLikeCost.h>
+#include <iDynTree/LinearCost.h>
 #include <iDynTree/TimeRange.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
@@ -42,12 +45,16 @@ namespace iDynTree {
             MatrixDynSize stateJacobianBuffer, controlJacobianBuffer;
         } BufferedGroup;
 
-        typedef std::map< std::string, BufferedGroup> ConstraintsGroupsMap;
+        typedef std::shared_ptr<BufferedGroup> BufferedGroup_ptr;
+
+        typedef std::map< std::string, BufferedGroup_ptr> ConstraintsGroupsMap;
 
         typedef struct{
             std::shared_ptr<Cost> cost;
             double weight;
             TimeRange timeRange;
+            bool isLinear;
+            bool isQuadratic;
         } TimedCost;
 
         typedef std::map< std::string, TimedCost> CostsMap;
@@ -65,6 +72,32 @@ namespace iDynTree {
             VectorDynSize stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
             std::vector<std::string> mayerCostnames;
             std::vector<TimeRange> constraintsTimeRanges, costTimeRanges;
+            std::vector<size_t> linearConstraintIndeces;
+
+            bool addCost(double weight, const TimeRange& timeRange, std::shared_ptr<Cost> cost, bool isLinear, bool isQuadratic, const std::string& methodName) {
+                if (!cost){
+                    reportError("OptimalControlProblem", methodName.c_str(), "Empty cost pointer");
+                    return false;
+                }
+
+                TimedCost newCost;
+                newCost.cost = cost;
+                newCost.weight = weight;
+                newCost.timeRange = timeRange;
+                newCost.isLinear = isLinear;
+                newCost.isLinear = isQuadratic;
+
+                std::pair<CostsMap::iterator, bool> costResult;
+                costResult = costs.insert(std::pair<std::string, TimedCost>(cost->name(), newCost));
+                if(!costResult.second){
+                    std::ostringstream errorMsg;
+                    errorMsg << "A cost named " << cost->name() <<" already exists.";
+                    reportError("OptimalControlProblem", methodName.c_str(), errorMsg.str().c_str());
+                    return false;
+                }
+                return true;
+            }
+
         };
 
 
@@ -147,16 +180,16 @@ namespace iDynTree {
                  return false;
             }
 
-            BufferedGroup newGroup;
-            newGroup.group_ptr = groupOfConstraints;
-            newGroup.constraintsBuffer.resize(groupOfConstraints->constraintsDimension());
+            BufferedGroup_ptr newGroup = std::make_shared<BufferedGroup>();
+            newGroup->group_ptr = groupOfConstraints;
+            newGroup->constraintsBuffer.resize(groupOfConstraints->constraintsDimension());
             if (m_pimpl->dynamicalSystem) {
-                newGroup.stateJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize()));
-                newGroup.controlJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize()));
+                newGroup->stateJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize()));
+                newGroup->controlJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize()));
             }
 
             std::pair< ConstraintsGroupsMap::iterator, bool> groupResult;
-            groupResult = m_pimpl->constraintsGroups.insert(std::pair< std::string, BufferedGroup>(groupOfConstraints->name(), newGroup));
+            groupResult = m_pimpl->constraintsGroups.insert(std::pair< std::string, BufferedGroup_ptr>(groupOfConstraints->name(), newGroup));
 
             if(!groupResult.second){
                 std::ostringstream errorMsg;
@@ -188,10 +221,10 @@ namespace iDynTree {
             return false;
         }
 
-        bool OptimalControlProblem::addContraint(std::shared_ptr<Constraint> newConstraint)
+        bool OptimalControlProblem::addConstraint(std::shared_ptr<Constraint> newConstraint)
         {
             if (!newConstraint){
-                reportError("OptimalControlProblem", "addContraint", "Invalid constraint pointer.");
+                reportError("OptimalControlProblem", "addConstraint", "Invalid constraint pointer.");
                 return false;
             }
 
@@ -199,7 +232,7 @@ namespace iDynTree {
                     std::make_shared<ConstraintsGroup>(newConstraint->name(),newConstraint->constraintSize());
 
             if(!dummyGroup){
-                reportError("OptimalControlProblem", "addContraint", "Failed in adding constraint.");
+                reportError("OptimalControlProblem", "addConstraint", "Failed in adding constraint.");
                 return false;
             }
 
@@ -209,12 +242,33 @@ namespace iDynTree {
             return addGroupOfConstraints(dummyGroup);
         }
 
+        bool OptimalControlProblem::addConstraint(std::shared_ptr<LinearConstraint> newConstraint)
+        {
+            if (!newConstraint){
+                reportError("OptimalControlProblem", "addConstraint", "Invalid constraint pointer.");
+                return false;
+            }
+
+            std::shared_ptr<ConstraintsGroup> dummyGroup =
+                    std::make_shared<ConstraintsGroup>(newConstraint->name(),newConstraint->constraintSize());
+
+            if(!dummyGroup){
+                reportError("OptimalControlProblem", "addConstraint", "Failed in adding constraint.");
+                return false;
+            }
+
+            if(!(dummyGroup->addConstraint(newConstraint, TimeRange::AnyTime()))){ //here is important to pass a LinearConstraint pointer
+                return false;
+            }
+            return addGroupOfConstraints(dummyGroup);
+        }
+
         bool OptimalControlProblem::removeConstraint(const std::string &name)
         {
             ConstraintsGroupsMap::iterator groupIterator;
             groupIterator = m_pimpl->constraintsGroups.find(name);
             if(groupIterator != m_pimpl->constraintsGroups.end()){
-                if(groupIterator->second.group_ptr->isAnyTimeGroup()){
+                if(groupIterator->second->group_ptr->isAnyTimeGroup()){
                     if(m_pimpl->constraintsGroups.erase(name)){
                         return true;
                     }
@@ -243,7 +297,20 @@ namespace iDynTree {
             unsigned int number = 0;
 
             for(auto group: m_pimpl->constraintsGroups){
-                number += group.second.group_ptr->numberOfConstraints();
+                number += group.second->group_ptr->numberOfConstraints();
+            }
+
+            return number;
+        }
+
+        unsigned int OptimalControlProblem::countLinearConstraints() const
+        {
+            unsigned int number = 0;
+
+            for(auto group: m_pimpl->constraintsGroups){
+                if (group.second->group_ptr->isLinearGroup()) {
+                    number += group.second->group_ptr->constraintsDimension();
+                }
             }
 
             return number;
@@ -254,7 +321,7 @@ namespace iDynTree {
             unsigned int dimension = 0;
 
             for (auto group: m_pimpl->constraintsGroups){
-                dimension += group.second.group_ptr->constraintsDimension();
+                dimension += group.second->group_ptr->constraintsDimension();
             }
             return dimension;
         }
@@ -265,7 +332,7 @@ namespace iDynTree {
             std::vector<std::string> temp;
 
             for(auto group: m_pimpl->constraintsGroups){
-                temp =  group.second.group_ptr->listConstraints();
+                temp =  group.second->group_ptr->listConstraints();
                 output.insert(output.end(), temp.begin(), temp.end());
             }
             return output;
@@ -276,7 +343,7 @@ namespace iDynTree {
             std::vector<std::string> output;
 
             for(auto group: m_pimpl->constraintsGroups){
-                output.insert(output.end(), group.second.group_ptr->numberOfConstraints(), group.second.group_ptr->name());
+                output.insert(output.end(), group.second->group_ptr->numberOfConstraints(), group.second->group_ptr->name());
             }
             return output;
         }
@@ -291,7 +358,7 @@ namespace iDynTree {
             size_t index = 0;
 
             for (auto group : m_pimpl->constraintsGroups){
-                std::vector<TimeRange> &groupTimeRanges = group.second.group_ptr->getTimeRanges();
+                std::vector<TimeRange> &groupTimeRanges = group.second->group_ptr->getTimeRanges();
                 for (size_t i = 0; i < groupTimeRanges.size(); ++i){
                     m_pimpl->constraintsTimeRanges[index] = groupTimeRanges[i];
                     ++index;
@@ -300,52 +367,78 @@ namespace iDynTree {
             return m_pimpl->constraintsTimeRanges;
         }
 
-        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<Cost> cost)
+        std::vector<size_t> &OptimalControlProblem::getLinearConstraintsIndeces() const
         {
-            if (!cost){
-                reportError("OptimalControlProblem", "addMayerTerm", "Empty cost pointer");
-                return false;
+            unsigned int nl = countLinearConstraints();
+
+            if (m_pimpl->linearConstraintIndeces.size() != nl)
+                m_pimpl->linearConstraintIndeces.resize(nl);
+
+            size_t vectorIndex = 0, constraintIndex = 0;
+
+            unsigned int nc = 0;
+            for (auto group : m_pimpl->constraintsGroups){
+                nc = group.second->group_ptr->constraintsDimension();
+
+                if (group.second->group_ptr->isLinearGroup()) {
+                    for (size_t i = 0; i < nc; ++i){
+                        m_pimpl->linearConstraintIndeces[vectorIndex] = constraintIndex + i;
+                        ++vectorIndex;
+                    }
+                }
+                constraintIndex += nc;
             }
-            TimedCost newCost;
-            newCost.cost = cost;
-            newCost.weight = weight;
-            newCost.timeRange.setTimeInterval(m_pimpl->horizon.endTime(), m_pimpl->horizon.endTime());
+            return m_pimpl->linearConstraintIndeces;
+        }
 
-            std::pair<CostsMap::iterator, bool> costResult;
-            costResult = m_pimpl->costs.insert(std::pair<std::string, TimedCost>(cost->name(), newCost));
-            if(!costResult.second){
-                std::ostringstream errorMsg;
-                errorMsg << "A cost named " << cost->name() <<" already exists.";
-                reportError("OptimalControlProblem", "addMayerTerm", errorMsg.str().c_str());
+        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<Cost> cost)
+        {
+            TimeRange newTimerange(m_pimpl->horizon.endTime(), m_pimpl->horizon.endTime());
+            if (!(m_pimpl->addCost(weight, newTimerange, cost, false, false, "addMayerTerm"))) {
                 return false;
             }
             m_pimpl->mayerCostnames.push_back(cost->name());
+
             return true;
         }
 
-        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<Cost> cost)
+        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost)
         {
-            if (!cost){
-                reportError("OptimalControlProblem", "addLagrangeTerm", "Empty cost pointer");
+            TimeRange newTimerange(m_pimpl->horizon.endTime(), m_pimpl->horizon.endTime());
+            if (!(m_pimpl->addCost(weight, newTimerange, quadraticCost, false, true, "addMayerTerm"))) {
                 return false;
             }
+            m_pimpl->mayerCostnames.push_back(quadraticCost->name());
 
-            TimedCost newCost;
-            newCost.cost = cost;
-            newCost.weight = weight;
-            newCost.timeRange = TimeRange::AnyTime();
+            return true;
+        }
 
-            std::pair<CostsMap::iterator, bool> costResult;
-            costResult = m_pimpl->costs.insert(std::pair<std::string, TimedCost>(cost->name(), newCost));
-            if(!costResult.second){
-                std::ostringstream errorMsg;
-                errorMsg << "A cost named " << cost->name() <<" already exists.";
-                reportError("OptimalControlProblem", "addLagrangeTerm", errorMsg.str().c_str());
+        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<LinearCost> linearCost)
+        {
+            TimeRange newTimerange(m_pimpl->horizon.endTime(), m_pimpl->horizon.endTime());
+            if (!(m_pimpl->addCost(weight, newTimerange, linearCost, true, true, "addMayerTerm"))) {
                 return false;
             }
+            m_pimpl->mayerCostnames.push_back(linearCost->name());
+
             return true;
         }
 
+        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<Cost> cost)
+        {
+            return (m_pimpl->addCost(weight, TimeRange::AnyTime(), cost, false, false, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost)
+        {
+            return (m_pimpl->addCost(weight, TimeRange::AnyTime(), quadraticCost, false, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<LinearCost> linearCost)
+        {
+            return (m_pimpl->addCost(weight, TimeRange::AnyTime(), linearCost, true, true, "addLagrangeTerm"));
+        }
+
         bool OptimalControlProblem::addLagrangeTerm(double weight, double startingTime, double finalTime, std::shared_ptr<Cost> cost)
         {
             TimeRange timeRange;
@@ -357,25 +450,50 @@ namespace iDynTree {
                 return false;
             }
 
-            return addLagrangeTerm(weight, timeRange, cost);
+            return (m_pimpl->addCost(weight, timeRange, cost, false, false, "addLagrangeTerm"));
         }
 
-        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<Cost> cost)
+        bool OptimalControlProblem::addLagrangeTerm(double weight, double startingTime, double finalTime, std::shared_ptr<QuadraticLikeCost> quadraticCost)
         {
-            TimedCost newCost;
-            newCost.cost = cost;
-            newCost.weight = weight;
-            newCost.timeRange = timeRange;
+            TimeRange timeRange;
 
-            std::pair<CostsMap::iterator, bool> costResult;
-            costResult = m_pimpl->costs.insert(std::pair<std::string, TimedCost>(cost->name(), newCost));
-            if(!costResult.second){
+            if (!timeRange.setTimeInterval(startingTime, finalTime)){
                 std::ostringstream errorMsg;
-                errorMsg << "A cost named " << cost->name() <<" already exists.";
+                errorMsg << "The cost named " << quadraticCost->name() <<" has invalid time settings.";
                 reportError("OptimalControlProblem", "addLagrangeTerm", errorMsg.str().c_str());
                 return false;
             }
-            return true;
+
+            return (m_pimpl->addCost(weight, timeRange, quadraticCost, false, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, double startingTime, double finalTime, std::shared_ptr<LinearCost> linearCost)
+        {
+            TimeRange timeRange;
+
+            if (!timeRange.setTimeInterval(startingTime, finalTime)){
+                std::ostringstream errorMsg;
+                errorMsg << "The cost named " << linearCost->name() <<" has invalid time settings.";
+                reportError("OptimalControlProblem", "addLagrangeTerm", errorMsg.str().c_str());
+                return false;
+            }
+
+            return (m_pimpl->addCost(weight, timeRange, linearCost, true, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<Cost> cost)
+        {
+            return (m_pimpl->addCost(weight, timeRange, cost, false, false, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<QuadraticLikeCost> quadraticCost)
+        {
+            return (m_pimpl->addCost(weight, timeRange, quadraticCost, false, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<LinearCost> linearCost)
+        {
+            return (m_pimpl->addCost(weight, timeRange, linearCost, true, true, "addLagrangeTerm"));
         }
 
         bool OptimalControlProblem::updateCostTimeRange(const std::string &name, double newStartingTime, double newEndTime)
@@ -445,6 +563,26 @@ namespace iDynTree {
             return m_pimpl->costTimeRanges;
         }
 
+        bool OptimalControlProblem::hasOnlyLinearCosts() const
+        {
+            for (auto c : m_pimpl->costs) {
+                if (!(c.second.isLinear)) {
+                    return false;
+                }
+            }
+            return true;
+        }
+
+        bool OptimalControlProblem::hasOnlyQuadraticCosts() const
+        {
+            for (auto c : m_pimpl->costs) {
+                if (!(c.second.isQuadratic)) {
+                    return false;
+                }
+            }
+            return true;
+        }
+
         bool OptimalControlProblem::setStateLowerBound(const VectorDynSize &minState)
         {
             if (!(m_pimpl->dynamicalSystem)){
@@ -774,15 +912,15 @@ namespace iDynTree {
             Eigen::Index offset = 0;
 
             for (auto group : m_pimpl->constraintsGroups){
-                if (!(group.second.group_ptr->evaluateConstraints(time, state, control, group.second.constraintsBuffer))){
+                if (!(group.second->group_ptr->evaluateConstraints(time, state, control, group.second->constraintsBuffer))){
                     std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating constraint " << group.second.group_ptr->name() <<".";
+                    errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
                     reportError("OptimalControlProblem", "constraintsEvaluation", errorMsg.str().c_str());
                     return false;
                 }
 
-                constraintsEvaluation.segment(offset, group.second.constraintsBuffer.size()) = toEigen(group.second.constraintsBuffer);
-                offset += group.second.constraintsBuffer.size();
+                constraintsEvaluation.segment(offset, group.second->constraintsBuffer.size()) = toEigen(group.second->constraintsBuffer);
+                offset += group.second->constraintsBuffer.size();
             }
 
             return true;
@@ -798,19 +936,19 @@ namespace iDynTree {
             Eigen::Index offset = 0;
 
             for (auto group : m_pimpl->constraintsGroups){
-                if (! group.second.group_ptr->getUpperBound(time, group.second.constraintsBuffer)){
-                    toEigen(group.second.constraintsBuffer).setConstant(std::abs(infinity)); //if not upper bounded
+                if (! group.second->group_ptr->getUpperBound(time, group.second->constraintsBuffer)){
+                    toEigen(group.second->constraintsBuffer).setConstant(std::abs(infinity)); //if not upper bounded
                 }
 
-                if (group.second.constraintsBuffer.size() != group.second.group_ptr->constraintsDimension()){
+                if (group.second->constraintsBuffer.size() != group.second->group_ptr->constraintsDimension()){
                     std::ostringstream errorMsg;
-                    errorMsg << "Upper bound dimension different from dimension of group " << group.second.group_ptr->name() << ".";
+                    errorMsg << "Upper bound dimension different from dimension of group " << group.second->group_ptr->name() << ".";
                     reportError("OptimalControlProblem", "getConstraintsUpperBound", errorMsg.str().c_str());
                     return false;
                 }
 
-                upperBoundMap.segment(offset, group.second.constraintsBuffer.size()) = toEigen(group.second.constraintsBuffer);
-                offset += group.second.constraintsBuffer.size();
+                upperBoundMap.segment(offset, group.second->constraintsBuffer.size()) = toEigen(group.second->constraintsBuffer);
+                offset += group.second->constraintsBuffer.size();
             }
 
             return true;
@@ -826,19 +964,19 @@ namespace iDynTree {
             Eigen::Index offset = 0;
 
             for (auto group : m_pimpl->constraintsGroups){
-                if (! group.second.group_ptr->getLowerBound(time, group.second.constraintsBuffer)){
-                    toEigen(group.second.constraintsBuffer).setConstant(-std::abs(infinity)); //if not lower bounded
+                if (! group.second->group_ptr->getLowerBound(time, group.second->constraintsBuffer)){
+                    toEigen(group.second->constraintsBuffer).setConstant(-std::abs(infinity)); //if not lower bounded
                 }
 
-                if (group.second.constraintsBuffer.size() != group.second.group_ptr->constraintsDimension()){
+                if (group.second->constraintsBuffer.size() != group.second->group_ptr->constraintsDimension()){
                     std::ostringstream errorMsg;
-                    errorMsg << "Lower bound dimension different from dimension of group " << group.second.group_ptr->name() << ".";
+                    errorMsg << "Lower bound dimension different from dimension of group " << group.second->group_ptr->name() << ".";
                     reportError("OptimalControlProblem", "getConstraintsUpperBound", errorMsg.str().c_str());
                     return false;
                 }
 
-                lowerBoundMap.segment(offset, group.second.constraintsBuffer.size()) = toEigen(group.second.constraintsBuffer);
-                offset += group.second.constraintsBuffer.size();
+                lowerBoundMap.segment(offset, group.second->constraintsBuffer.size()) = toEigen(group.second->constraintsBuffer);
+                offset += group.second->constraintsBuffer.size();
             }
 
             return true;
@@ -847,7 +985,7 @@ namespace iDynTree {
         bool OptimalControlProblem::isFeasiblePoint(double time, const VectorDynSize &state, const VectorDynSize &control)
         {
             for(auto group : m_pimpl->constraintsGroups){
-                if(!(group.second.group_ptr->isFeasibilePoint(time, state, control))){
+                if(!(group.second->group_ptr->isFeasibilePoint(time, state, control))){
                     return false;
                 }
             }
@@ -865,15 +1003,15 @@ namespace iDynTree {
             Eigen::Index offset = 0;
 
             for (auto group : m_pimpl->constraintsGroups){
-                if (!(group.second.group_ptr->constraintJacobianWRTState(time, state, control, group.second.stateJacobianBuffer))){
+                if (!(group.second->group_ptr->constraintJacobianWRTState(time, state, control, group.second->stateJacobianBuffer))){
                     std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating constraint group " << group.second.group_ptr->name() <<".";
+                    errorMsg << "Error while evaluating constraint group " << group.second->group_ptr->name() <<".";
                     reportError("OptimalControlProblem", "constraintsJacobianWRTState", errorMsg.str().c_str());
                     return false;
                 }
 
-                jacobianMap.block(offset, 0, group.second.group_ptr->constraintsDimension(), state.size()) = toEigen(group.second.stateJacobianBuffer);
-                offset += group.second.stateJacobianBuffer.rows();
+                jacobianMap.block(offset, 0, group.second->group_ptr->constraintsDimension(), state.size()) = toEigen(group.second->stateJacobianBuffer);
+                offset += group.second->stateJacobianBuffer.rows();
             }
             return true;
         }
@@ -889,14 +1027,14 @@ namespace iDynTree {
             Eigen::Index offset = 0;
 
             for (auto group : m_pimpl->constraintsGroups){
-                if (! group.second.group_ptr->constraintJacobianWRTControl(time, state, control, group.second.controlJacobianBuffer)){
+                if (! group.second->group_ptr->constraintJacobianWRTControl(time, state, control, group.second->controlJacobianBuffer)){
                     std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating constraint " << group.second.group_ptr->name() <<".";
+                    errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
                     reportError("OptimalControlProblem", "constraintsJacobianWRTControl", errorMsg.str().c_str());
                     return false;
                 }
-                jacobianMap.block(offset, 0, group.second.group_ptr->constraintsDimension(), control.size()) = toEigen(group.second.controlJacobianBuffer);
-                offset += group.second.controlJacobianBuffer.rows();
+                jacobianMap.block(offset, 0, group.second->group_ptr->constraintsDimension(), control.size()) = toEigen(group.second->controlJacobianBuffer);
+                offset += group.second->controlJacobianBuffer.rows();
             }
             return true;
         }
diff --git a/src/optimalcontrol/tests/IntegratorsTest.cpp b/src/optimalcontrol/tests/IntegratorsTest.cpp
index d0accdf1641..d1a47b89cd6 100644
--- a/src/optimalcontrol/tests/IntegratorsTest.cpp
+++ b/src/optimalcontrol/tests/IntegratorsTest.cpp
@@ -292,19 +292,19 @@ int main(){
     ASSERT_IS_TRUE(direct.evaluateCollocationConstraint(0.0, c1, c2, dT, v3)); //for eventual memory allocation
     ASSERT_IS_TRUE(ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
 
+    initT = clock();
     for (int i = 0; i < 10000; ++i) {
-        initT = clock();
-        ASSERT_IS_TRUE(direct.evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
-        endT = clock();
-        sumDirect += (endT-initT);
-
-        initT = clock();
-        ASSERT_IS_TRUE(ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3));
-        endT = clock();
-        sumPtr += (endT-initT);
+        ptr->evaluateCollocationConstraint(0.0, c1, c2, dT, v3);
     }
-    std::cerr << "Elapsed time (direct): " <<  static_cast<double>(sumDirect) *1000 / CLOCKS_PER_SEC <<" ms."<<std::endl;
-    std::cerr << "Elapsed time (ptr): " <<  static_cast<double>(sumPtr) *1000 / CLOCKS_PER_SEC <<" ms."<<std::endl; //Multiplying first to avoid decimation issues
+    endT = clock();
+    std::cerr << "Elapsed time (ptr): " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+
+    initT = clock();
+    for (int i = 0; i < 10000; ++i) {
+        direct.evaluateCollocationConstraint(0.0, c1, c2, dT, v3);
+    }
+    endT = clock();
+    std::cerr << "Elapsed time (direct): " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
 
     return EXIT_SUCCESS;
 }
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 915b9f2ad9b..9fe621a4152 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -368,7 +368,7 @@ int main(){
     ASSERT_IS_TRUE(!(problem->dynamicalSystem().expired()));
     ASSERT_IS_TRUE(problem->addGroupOfConstraints(group1));
     ASSERT_IS_TRUE(group1->addConstraint(constraint2, iDynTree::optimalcontrol::TimeRange(4.0, 5.0)));
-    ASSERT_IS_TRUE(problem->addContraint(constraint1));
+    ASSERT_IS_TRUE(problem->addConstraint(constraint1));
     ASSERT_IS_TRUE(problem->addLagrangeTerm(1.0, cost1));
     ASSERT_IS_TRUE(problem->addMayerTerm(1.0, cost2));
 
diff --git a/src/optimalcontrol/tests/OCProblemTest.cpp b/src/optimalcontrol/tests/OCProblemTest.cpp
index 9862ab102ee..12263557df4 100644
--- a/src/optimalcontrol/tests/OCProblemTest.cpp
+++ b/src/optimalcontrol/tests/OCProblemTest.cpp
@@ -277,7 +277,7 @@ int main() {
     ASSERT_IS_TRUE(!(problem.dynamicalSystem().expired()));
     ASSERT_IS_TRUE(problem.addGroupOfConstraints(group1));
     ASSERT_IS_TRUE(group1->addConstraint(constraint2, iDynTree::optimalcontrol::TimeRange(4.0, 5.0)));
-    ASSERT_IS_TRUE(problem.addContraint(constraint1));
+    ASSERT_IS_TRUE(problem.addConstraint(constraint1));
     ASSERT_IS_TRUE(problem.addLagrangeTerm(1.0, cost1));
     ASSERT_IS_TRUE(problem.addMayerTerm(1.0, cost2));
 

From 347fd70bfdee224b63fd9013e266081234bb9b96 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Jun 2018 14:59:34 +0200
Subject: [PATCH 021/194] A linear system can be added to the
 OptimalControlProblem.

---
 .../include/iDynTree/DynamicalSystem.h         |  2 +-
 .../include/iDynTree/OptimalControlProblem.h   |  8 ++++++++
 .../src/OptimalControlProblem.cpp              | 18 ++++++++++++++++++
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index 435dc5b2fbb..811169ca65c 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -48,6 +48,7 @@ namespace optimalcontrol {
         virtual ~DynamicalSystem();
 
         size_t stateSpaceSize() const;
+
         size_t controlSpaceSize() const;
 
         virtual bool dynamics(const VectorDynSize& state,
@@ -76,7 +77,6 @@ namespace optimalcontrol {
                                                     MatrixDynSize& dynamicsDerivative);
 
     private:
-        // TODO: does it make sense to have members in this abstrac class?
         size_t m_stateSize;
         size_t m_controlSize;
         VectorDynSize m_initialState;
diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index 57a55cddf21..5161d6a1ac8 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -31,6 +31,7 @@ namespace iDynTree {
     namespace optimalcontrol {
 
         class DynamicalSystem;
+        class LinearSystem;
         class Constraint;
         class LinearConstraint;
         class ConstraintsGroup;
@@ -54,12 +55,19 @@ namespace iDynTree {
             OptimalControlProblem(const OptimalControlProblem& other) = delete;
 
             bool setTimeHorizon(double startingTime, double finalTime);
+
             double initialTime() const;
+
             double finalTime() const;
 
             bool setDynamicalSystemConstraint(std::shared_ptr<DynamicalSystem> dynamicalSystem);
+
+            bool setDynamicalSystemConstraint(std::shared_ptr<LinearSystem> linearSystem);
+
             const std::weak_ptr<DynamicalSystem> dynamicalSystem() const;
 
+            bool systemIsLinear() const;
+
             bool addGroupOfConstraints(std::shared_ptr<ConstraintsGroup> groupOfConstraints); //to be used when the constraints applies only for a time interval
 
             bool removeGroupOfConstraints(const std::string& name);
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 073de5c13da..359c4f7ffd0 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -16,6 +16,7 @@
 
 #include <iDynTree/OptimalControlProblem.h>
 #include <iDynTree/DynamicalSystem.h>
+#include <iDynTree/LinearSystem.h>
 #include <iDynTree/ConstraintsGroup.h>
 #include <iDynTree/Constraint.h>
 #include <iDynTree/LinearConstraint.h>
@@ -73,6 +74,7 @@ namespace iDynTree {
             std::vector<std::string> mayerCostnames;
             std::vector<TimeRange> constraintsTimeRanges, costTimeRanges;
             std::vector<size_t> linearConstraintIndeces;
+            bool systemIsLinear = false;
 
             bool addCost(double weight, const TimeRange& timeRange, std::shared_ptr<Cost> cost, bool isLinear, bool isQuadratic, const std::string& methodName) {
                 if (!cost){
@@ -168,11 +170,27 @@ namespace iDynTree {
             return true;
         }
 
+        bool OptimalControlProblem::setDynamicalSystemConstraint(std::shared_ptr<LinearSystem> linearSystem)
+        {
+            if (m_pimpl->dynamicalSystem){
+                reportError("OptimalControlProblem", "setDynamicalSystemConstraint", "Change dynamical system is forbidden.");
+                return false;
+            }
+            m_pimpl->dynamicalSystem = linearSystem;
+            m_pimpl->systemIsLinear = true;
+            return true;
+        }
+
         const std::weak_ptr<DynamicalSystem> OptimalControlProblem::dynamicalSystem() const
         {
             return m_pimpl->dynamicalSystem;
         }
 
+        bool OptimalControlProblem::systemIsLinear() const
+        {
+            return m_pimpl->systemIsLinear;
+        }
+
         bool OptimalControlProblem::addGroupOfConstraints(std::shared_ptr<ConstraintsGroup> groupOfConstraints)
         {
             if (!groupOfConstraints){

From e10f0990e45cad79ebdd69635787c2bf36877c81 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Jun 2018 17:01:20 +0200
Subject: [PATCH 022/194] Hidden MultipleShootingTranscription definition.

---
 .../OCSolvers/MultipleShootingSolver.h        |   83 +-
 .../src/MultipleShootingSolver.cpp            | 2213 ++++++++---------
 2 files changed, 1094 insertions(+), 1202 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
index ade0b310859..fc21d818c0d 100644
--- a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
+++ b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
@@ -42,86 +42,6 @@ namespace iDynTree {
          * \ingroup iDynTreeExperimental
          */
 
-        class MultipleShootingTranscription : public optimization::OptimizationProblem {
-
-            friend class MultipleShootingSolver;
-
-            MultipleShootingTranscription();
-
-            MultipleShootingTranscription(const std::shared_ptr<OptimalControlProblem> problem, const std::shared_ptr<Integrator> integrationMethod);
-
-            MultipleShootingTranscription(const MultipleShootingTranscription& other) = delete;
-
-            size_t setControlMeshPoints();
-
-            bool preliminaryChecks();
-
-            bool setMeshPoints();
-
-            bool setOptimalControlProblem(const std::shared_ptr<OptimalControlProblem> problem);
-
-            bool setIntegrator(const std::shared_ptr<Integrator> integrationMethod);
-
-            bool setStepSizeBounds(const double minStepSize, const double maxStepsize);
-
-            bool setControlPeriod(double period);
-
-            bool setAdditionalStateMeshPoints(const std::vector<double>& stateMeshes);
-
-            bool setAdditionalControlMeshPoints(const std::vector<double>& controlMeshes);
-
-            void setPlusInfinity(double plusInfinity);
-
-            void setMinusInfinity(double minusInfinity);
-
-            bool setInitialState(const VectorDynSize &initialState);
-
-            bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations);
-
-            bool getSolution(std::vector<VectorDynSize>& states, std::vector<VectorDynSize>& controls);
-
-            class MultipleShootingTranscriptionPimpl;
-            MultipleShootingTranscriptionPimpl *m_pimpl;
-
-        public:
-
-            virtual ~MultipleShootingTranscription() override;
-
-            virtual bool prepare() override;
-
-            virtual void reset() override;
-
-            virtual unsigned int numberOfVariables() override;
-
-            virtual unsigned int numberOfConstraints() override;
-
-            virtual bool getConstraintsBounds(VectorDynSize& constraintsLowerBounds, VectorDynSize& constraintsUpperBounds) override;
-
-            virtual bool getVariablesUpperBound(VectorDynSize& variablesUpperBound) override;
-
-            virtual bool getVariablesLowerBound(VectorDynSize& variablesLowerBound) override;
-
-            virtual bool getConstraintsJacobianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override;
-
-            virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override;
-
-            virtual bool setVariables(const VectorDynSize& variables) override;
-
-            virtual bool evaluateCostFunction(double& costValue) override;
-
-            virtual bool evaluateCostGradient(VectorDynSize& gradient) override;
-
-            virtual bool evaluateCostHessian(MatrixDynSize& hessian) override; //using dense matrices, but the sparsity pattern is still obtained
-
-            virtual bool evaluateConstraints(VectorDynSize& constraints) override;
-
-            virtual bool evaluateConstraintsJacobian(MatrixDynSize& jacobian) override; //using dense matrices, but the sparsity pattern is still obtained
-
-            virtual bool evaluateConstraintsHessian(const VectorDynSize& constraintsMultipliers, MatrixDynSize& hessian) override; //using dense matrices, but the sparsity pattern is still obtained
-
-        };
-
-
         class MultipleShootingSolver : public OptimalControlSolver {
 
         public:
@@ -153,6 +73,9 @@ namespace iDynTree {
 
 
         private:
+
+            class MultipleShootingTranscription;
+
             std::shared_ptr<MultipleShootingTranscription> m_transcription;
             std::shared_ptr<optimization::Optimizer> m_optimizer;
         };
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 13cf19a2a11..e168c9811f1 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -82,65 +82,69 @@ namespace iDynTree {
             //std::vector<size_t> integratorAuxiliariesOffsets;
         } MeshPoint;
 
-        class MultipleShootingTranscription::MultipleShootingTranscriptionPimpl{
-        public:
-            std::shared_ptr<OptimalControlProblem> ocproblem;
-            std::shared_ptr<Integrator> integrator;
-            size_t totalMeshes, controlMeshes;
-            bool prepared;
-            std::vector<double> userStateMeshes, userControlMeshes;
-            std::vector<MeshPoint> meshPoints;
-            std::vector<MeshPoint>::iterator meshPointsEnd;
-            double minStepSize, maxStepSize, controlPeriod;
-            size_t nx, nu, numberOfVariables, constraintsPerInstant, numberOfConstraints;
-            std::vector<size_t> jacobianNZRows, jacobianNZCols, hessianNZRows, hessianNZCols;
-            size_t jacobianNonZeros, hessianNonZeros;
-            double plusInfinity, minusInfinity;
-            VectorDynSize constraintsLowerBound, constraintsUpperBound;
-            VectorDynSize constraintsBuffer, stateBuffer, controlBuffer, variablesBuffer, costStateGradientBuffer, costControlGradientBuffer;
-            MatrixDynSize costHessianStateBuffer, costHessianControlBuffer, costHessianStateControlBuffer, costHessianControlStateBuffer;
-            std::vector<VectorDynSize> collocationStateBuffer, collocationControlBuffer;
-            std::vector<MatrixDynSize> collocationStateJacBuffer, collocationControlJacBuffer;
-            MatrixDynSize constraintsStateJacBuffer, constraintsControlJacBuffer;
-            VectorDynSize solution;
-            bool solved;
+        //MARK: Transcription implementation
+
+        class MultipleShootingSolver::MultipleShootingTranscription : public optimization::OptimizationProblem {
+
+            std::shared_ptr<OptimalControlProblem> m_ocproblem;
+            std::shared_ptr<Integrator> m_integrator;
+            size_t m_totalMeshes, m_controlMeshes;
+            bool m_prepared;
+            std::vector<double> m_userStateMeshes, m_userControlMeshes;
+            std::vector<MeshPoint> m_meshPoints;
+            std::vector<MeshPoint>::iterator m_meshPointsEnd;
+            double m_minStepSize, m_maxStepSize, m_controlPeriod;
+            size_t m_nx, m_nu, m_numberOfVariables, m_constraintsPerInstant, m_numberOfConstraints;
+            std::vector<size_t> m_jacobianNZRows, m_jacobianNZCols, m_hessianNZRows, m_hessianNZCols;
+            size_t m_jacobianNonZeros, m_hessianNonZeros;
+            double m_plusInfinity, m_minusInfinity;
+            VectorDynSize m_constraintsLowerBound, m_constraintsUpperBound;
+            VectorDynSize m_constraintsBuffer, m_stateBuffer, m_controlBuffer, m_variablesBuffer, m_costStateGradientBuffer, m_costControlGradientBuffer;
+            MatrixDynSize m_costHessianStateBuffer, m_costHessianControlBuffer, m_costHessianStateControlBuffer, m_costHessianControlStateBuffer;
+            std::vector<VectorDynSize> m_collocationStateBuffer, m_collocationControlBuffer;
+            std::vector<MatrixDynSize> m_collocationStateJacBuffer, m_collocationControlJacBuffer;
+            MatrixDynSize m_constraintsStateJacBuffer, m_constraintsControlJacBuffer;
+            VectorDynSize m_solution;
+            bool m_solved;
+
+            friend class MultipleShootingSolver;
 
             void resetMeshPoints(){
-                meshPointsEnd = meshPoints.begin();
+                m_meshPointsEnd = m_meshPoints.begin();
             }
 
             void addMeshPoint(MeshPoint& newMeshPoint){
-                if (meshPointsEnd == meshPoints.end()){
-                    meshPoints.push_back(newMeshPoint);
-                    meshPointsEnd = meshPoints.end();
+                if (m_meshPointsEnd == m_meshPoints.end()){
+                    m_meshPoints.push_back(newMeshPoint);
+                    m_meshPointsEnd = m_meshPoints.end();
                 } else {
-                    *meshPointsEnd = newMeshPoint;
-                    ++meshPointsEnd;
+                    *m_meshPointsEnd = newMeshPoint;
+                    ++m_meshPointsEnd;
                 }
             }
 
             std::vector<MeshPoint>::iterator findNextMeshPoint(std::vector<MeshPoint>::iterator& start){
                 std::vector<MeshPoint>::iterator nextMesh = start;
                 MeshPointOrigin ignored = MeshPointOrigin::Ignored();
-                assert(nextMesh != meshPoints.end());
+                assert(nextMesh != m_meshPoints.end());
                 assert(nextMesh->origin.name().size() > 0);
                 do {
                     ++nextMesh;
                     assert(nextMesh->origin.name().size() > 0);
-                    assert(nextMesh != meshPoints.end());
+                    assert(nextMesh != m_meshPoints.end());
                 } while (nextMesh->origin == ignored); //find next valid mesh
 
                 return nextMesh;
             }
 
             void setIgnoredMesh(std::vector<MeshPoint>::iterator& toBeIgnored){
-                assert(toBeIgnored != meshPoints.end());
-                toBeIgnored->time = ocproblem->finalTime() + maxStepSize;
+                assert(toBeIgnored != m_meshPoints.end());
+                toBeIgnored->time = m_ocproblem->finalTime() + m_maxStepSize;
                 toBeIgnored->origin = MeshPointOrigin::Ignored();
             }
 
             void cleanLeftoverMeshes(){
-                for (auto toBeIgnored = meshPointsEnd; toBeIgnored != meshPoints.end(); toBeIgnored++){
+                for (auto toBeIgnored = m_meshPointsEnd; toBeIgnored != m_meshPoints.end(); toBeIgnored++){
                     setIgnoredMesh(toBeIgnored);
                 }
             }
@@ -155,8 +159,8 @@ namespace iDynTree {
             }
 
             void resetNonZerosCount(){
-                jacobianNonZeros = 0;
-                hessianNonZeros = 0;
+                m_jacobianNonZeros = 0;
+                m_hessianNonZeros = 0;
             }
 
             void addNonZero(std::vector<size_t>& input, size_t position, size_t toBeAdded){
@@ -171,9 +175,9 @@ namespace iDynTree {
             void addJacobianBlock(size_t initRow, size_t rows, size_t initCol, size_t cols){
                 for (size_t i = 0; i < rows; ++i){
                     for (size_t j = 0; j < cols; ++j){
-                        addNonZero(jacobianNZRows, jacobianNonZeros, initRow + i);
-                        addNonZero(jacobianNZCols, jacobianNonZeros, initCol + j);
-                        jacobianNonZeros++;
+                        addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + i);
+                        addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + j);
+                        m_jacobianNonZeros++;
                     }
                 }
             }
@@ -181,1427 +185,1392 @@ namespace iDynTree {
             void addHessianBlock(size_t initRow, size_t rows, size_t initCol, size_t cols){
                 for (size_t i = 0; i < rows; ++i){
                     for (size_t j = 0; j < cols; ++j){
-                        addNonZero(hessianNZRows, hessianNonZeros, initRow + i);
-                        addNonZero(hessianNZCols, hessianNonZeros, initCol + j);
-                        hessianNonZeros++;
+                        addNonZero(m_hessianNZRows, m_hessianNonZeros, initRow + i);
+                        addNonZero(m_hessianNZCols, m_hessianNonZeros, initCol + j);
+                        m_hessianNonZeros++;
                     }
                 }
             }
 
             void allocateBuffers(){
-                if (stateBuffer.size() != nx) {
-                    stateBuffer.resize(static_cast<unsigned int>(nx));
+                if (m_stateBuffer.size() != m_nx) {
+                    m_stateBuffer.resize(static_cast<unsigned int>(m_nx));
                 }
 
-                if (costStateGradientBuffer.size() != nx) {
-                    costStateGradientBuffer.resize(static_cast<unsigned int>(nx));
+                if (m_costStateGradientBuffer.size() != m_nx) {
+                    m_costStateGradientBuffer.resize(static_cast<unsigned int>(m_nx));
                 }
 
-                if ((costHessianStateBuffer.rows() != nx) || (costHessianStateBuffer.cols() != nx)) {
-                    costHessianStateBuffer.resize(static_cast<unsigned int>(nx), static_cast<unsigned int>(nx));
+                if ((m_costHessianStateBuffer.rows() != m_nx) || (m_costHessianStateBuffer.cols() != m_nx)) {
+                    m_costHessianStateBuffer.resize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nx));
                 }
 
-                if (controlBuffer.size() != nu) {
-                    controlBuffer.resize(static_cast<unsigned int>(nu));
+                if (m_controlBuffer.size() != m_nu) {
+                    m_controlBuffer.resize(static_cast<unsigned int>(m_nu));
                 }
 
-                if (costControlGradientBuffer.size() != nu) {
-                    costControlGradientBuffer.resize(static_cast<unsigned int>(nu));
+                if (m_costControlGradientBuffer.size() != m_nu) {
+                    m_costControlGradientBuffer.resize(static_cast<unsigned int>(m_nu));
                 }
 
-                if ((costHessianControlBuffer.rows() != nu) || (costHessianControlBuffer.cols() != nu)) {
-                    costHessianControlBuffer.resize(static_cast<unsigned int>(nu), static_cast<unsigned int>(nu));
+                if ((m_costHessianControlBuffer.rows() != m_nu) || (m_costHessianControlBuffer.cols() != m_nu)) {
+                    m_costHessianControlBuffer.resize(static_cast<unsigned int>(m_nu), static_cast<unsigned int>(m_nu));
                 }
 
-                if ((costHessianStateControlBuffer.rows() != nx) || (costHessianStateControlBuffer.cols() != nu)) {
-                    costHessianStateControlBuffer.resize(static_cast<unsigned int>(nx), static_cast<unsigned int>(nu));
+                if ((m_costHessianStateControlBuffer.rows() != m_nx) || (m_costHessianStateControlBuffer.cols() != m_nu)) {
+                    m_costHessianStateControlBuffer.resize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
                 }
 
-                if ((costHessianControlStateBuffer.rows() != nu) || (costHessianControlStateBuffer.cols() != nx)) {
-                    costHessianControlStateBuffer.resize(static_cast<unsigned int>(nu), static_cast<unsigned int>(nx));
+                if ((m_costHessianControlStateBuffer.rows() != m_nu) || (m_costHessianControlStateBuffer.cols() != m_nx)) {
+                    m_costHessianControlStateBuffer.resize(static_cast<unsigned int>(m_nu), static_cast<unsigned int>(m_nx));
                 }
 
 
                 //TODO: I should consider also the possibility to have auxiliary variables in the integrator
-                if (variablesBuffer.size() != numberOfVariables) {
-                    variablesBuffer.resize(static_cast<unsigned int>(numberOfVariables));
+                if (m_variablesBuffer.size() != m_numberOfVariables) {
+                    m_variablesBuffer.resize(static_cast<unsigned int>(m_numberOfVariables));
                 }
 
-                if (constraintsBuffer.size() != constraintsPerInstant) {
-                    constraintsBuffer.resize(static_cast<unsigned int>(constraintsPerInstant));
+                if (m_constraintsBuffer.size() != m_constraintsPerInstant) {
+                    m_constraintsBuffer.resize(static_cast<unsigned int>(m_constraintsPerInstant));
                 }
 
-                if (constraintsLowerBound.size() != numberOfConstraints) {
-                    constraintsLowerBound.resize(static_cast<unsigned int>(numberOfConstraints));
+                if (m_constraintsLowerBound.size() != m_numberOfConstraints) {
+                    m_constraintsLowerBound.resize(static_cast<unsigned int>(m_numberOfConstraints));
                 }
 
-                if (constraintsUpperBound.size() != numberOfConstraints) {
-                    constraintsUpperBound.resize(static_cast<unsigned int>(numberOfConstraints));
+                if (m_constraintsUpperBound.size() != m_numberOfConstraints) {
+                    m_constraintsUpperBound.resize(static_cast<unsigned int>(m_numberOfConstraints));
                 }
 
                 //TODO: I should consider also the possibility to have auxiliary variables in the integrator
-                if (collocationStateBuffer.size() != 2) {
-                    collocationStateBuffer.resize(2);
+                if (m_collocationStateBuffer.size() != 2) {
+                    m_collocationStateBuffer.resize(2);
                 }
                 for (size_t i = 0; i < 2; ++i) {
-                    if (collocationStateBuffer[i].size() != nx) {
-                        collocationStateBuffer[i].resize(static_cast<unsigned int>(nx));
+                    if (m_collocationStateBuffer[i].size() != m_nx) {
+                        m_collocationStateBuffer[i].resize(static_cast<unsigned int>(m_nx));
                     }
                 }
 
-                if (collocationControlBuffer.size() != 2) {
-                    collocationControlBuffer.resize(2);
+                if (m_collocationControlBuffer.size() != 2) {
+                    m_collocationControlBuffer.resize(2);
                 }
                 for (size_t i = 0; i < 2; ++i) {
-                    if (collocationControlBuffer[i].size() != nu) {
-                        collocationControlBuffer[i].resize(static_cast<unsigned int>(nu));
+                    if (m_collocationControlBuffer[i].size() != m_nu) {
+                        m_collocationControlBuffer[i].resize(static_cast<unsigned int>(m_nu));
                     }
                 }
 
-                if (collocationStateJacBuffer.size() != 2) {
-                    collocationStateJacBuffer.resize(2);
+                if (m_collocationStateJacBuffer.size() != 2) {
+                    m_collocationStateJacBuffer.resize(2);
                 }
                 for (size_t i = 0; i < 2; ++i) {
-                    if ((collocationStateJacBuffer[i].rows() != nx) || (collocationStateJacBuffer[i].cols() != nx)) {
-                        collocationStateJacBuffer[i].resize(static_cast<unsigned int>(nx), static_cast<unsigned int>(nx));
+                    if ((m_collocationStateJacBuffer[i].rows() != m_nx) || (m_collocationStateJacBuffer[i].cols() != m_nx)) {
+                        m_collocationStateJacBuffer[i].resize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nx));
                     }
                 }
 
-                if (collocationControlJacBuffer.size() != 2) {
-                    collocationControlJacBuffer.resize(2);
+                if (m_collocationControlJacBuffer.size() != 2) {
+                    m_collocationControlJacBuffer.resize(2);
                 }
                 for (size_t i = 0; i < 2; ++i) {
-                    if ((collocationControlJacBuffer[i].rows() != nx) || (collocationControlJacBuffer[i].cols() != nu)) {
-                        collocationControlJacBuffer[i].resize(static_cast<unsigned int>(nx), static_cast<unsigned int>(nu));
+                    if ((m_collocationControlJacBuffer[i].rows() != m_nx) || (m_collocationControlJacBuffer[i].cols() != m_nu)) {
+                        m_collocationControlJacBuffer[i].resize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
                     }
                 }
 
-                if ((constraintsStateJacBuffer.rows() != constraintsPerInstant) || (constraintsStateJacBuffer.cols() != nx)) {
-                    constraintsStateJacBuffer.resize(static_cast<unsigned int>(constraintsPerInstant), static_cast<unsigned int>(nx));
+                if ((m_constraintsStateJacBuffer.rows() != m_constraintsPerInstant) || (m_constraintsStateJacBuffer.cols() != m_nx)) {
+                    m_constraintsStateJacBuffer.resize(static_cast<unsigned int>(m_constraintsPerInstant), static_cast<unsigned int>(m_nx));
                 }
 
-                if ((constraintsControlJacBuffer.rows() != constraintsPerInstant) || (constraintsControlJacBuffer.cols() != nu)) {
-                    constraintsStateJacBuffer.resize(static_cast<unsigned int>(constraintsPerInstant), static_cast<unsigned int>(nu));
+                if ((m_constraintsControlJacBuffer.rows() != m_constraintsPerInstant) || (m_constraintsControlJacBuffer.cols() != m_nu)) {
+                    m_constraintsStateJacBuffer.resize(static_cast<unsigned int>(m_constraintsPerInstant), static_cast<unsigned int>(m_nu));
                 }
             }
 
 
+            size_t setControlMeshPoints() {
+                double endTime = m_ocproblem->finalTime(), initTime = m_ocproblem->initialTime();
+                size_t controlMeshes = 0;
+                double time = initTime;
+                MeshPoint newMeshPoint;
 
-            MultipleShootingTranscriptionPimpl()
-            : ocproblem(nullptr)
-            ,integrator(nullptr)
-            ,totalMeshes(0)
-            ,controlMeshes(0)
-            ,prepared(false)
-            ,meshPointsEnd(meshPoints.end())
-            ,minStepSize(0.001)
-            ,maxStepSize(0.01)
-            ,controlPeriod(0.01)
-            ,nx(0)
-            ,nu(0)
-            ,numberOfVariables(0)
-            ,constraintsPerInstant(0)
-            ,numberOfConstraints(0)
-            ,plusInfinity(1e19)
-            ,minusInfinity(-1e19)
-            ,solved(false)
-            {}
-
-            MultipleShootingTranscriptionPimpl(const std::shared_ptr<OptimalControlProblem> problem,
-                                            const std::shared_ptr<Integrator> integrationMethod)
-            :ocproblem(problem)
-            ,integrator(integrationMethod)
-            ,totalMeshes(0)
-            ,controlMeshes(0)
-            ,prepared(false)
-            ,meshPointsEnd(meshPoints.end())
-            ,minStepSize(0.001)
-            ,maxStepSize(0.01)
-            ,controlPeriod(0.01)
-            ,nx(0)
-            ,nu(0)
-            ,numberOfVariables(0)
-            ,constraintsPerInstant(0)
-            ,numberOfConstraints(0)
-            ,plusInfinity(1e19)
-            ,minusInfinity(-1e19)
-            ,solved(false)
-            {}
-        };
-
-        MultipleShootingTranscription::MultipleShootingTranscription()
-        :m_pimpl(new MultipleShootingTranscriptionPimpl())
-        {
-            assert(m_pimpl);
-        }
+                newMeshPoint.type = MeshPointType::Control;
 
-        MultipleShootingTranscription::MultipleShootingTranscription(const std::shared_ptr<OptimalControlProblem> problem,
-                                                               const std::shared_ptr<Integrator> integrationMethod)
-        :m_pimpl(new MultipleShootingTranscriptionPimpl(problem, integrationMethod))
-        {
-            assert(m_pimpl);
-        }
+                while (time <= endTime){
+                    if (checkDoublesAreEqual(time,initTime))//(time == initTime)
+                        newMeshPoint.origin = MeshPointOrigin::FirstPoint();
+                    else if (checkDoublesAreEqual(time,endTime))//(time == endTime)
+                        newMeshPoint.origin = MeshPointOrigin::LastPoint();
+                    else
+                        newMeshPoint.origin = MeshPointOrigin::Control();
+                    newMeshPoint.time = time;
 
-        size_t MultipleShootingTranscription::setControlMeshPoints()
-        {
-            double endTime = m_pimpl->ocproblem->finalTime(), initTime = m_pimpl->ocproblem->initialTime();
-            size_t controlMeshes = 0;
-            double time = initTime;
-            MeshPoint newMeshPoint;
-
-            newMeshPoint.type = MeshPointType::Control;
-
-            while (time <= endTime){
-                if (checkDoublesAreEqual(time,initTime))//(time == initTime)
-                    newMeshPoint.origin = MeshPointOrigin::FirstPoint();
-                else if (checkDoublesAreEqual(time,endTime))//(time == endTime)
-                    newMeshPoint.origin = MeshPointOrigin::LastPoint();
-                else
-                    newMeshPoint.origin = MeshPointOrigin::Control();
-                newMeshPoint.time = time;
-
-                m_pimpl->addMeshPoint(newMeshPoint);
-                time += m_pimpl->controlPeriod;
-                controlMeshes++;
-            }
+                    addMeshPoint(newMeshPoint);
+                    time += m_controlPeriod;
+                    controlMeshes++;
+                }
 
-            std::vector<MeshPoint>::iterator lastControlMeshIterator = (m_pimpl->meshPointsEnd)-1;
+                std::vector<MeshPoint>::iterator lastControlMeshIterator = (m_meshPointsEnd)-1;
 
-            if ((lastControlMeshIterator->origin == MeshPointOrigin::LastPoint()) && (m_pimpl->integrator->info().isExplicit())) {//the last control input would have no effect
-                controlMeshes--;
-                m_pimpl->setIgnoredMesh(lastControlMeshIterator);
-            }
+                if ((lastControlMeshIterator->origin == MeshPointOrigin::LastPoint()) && (m_integrator->info().isExplicit())) {//the last control input would have no effect
+                    controlMeshes--;
+                    setIgnoredMesh(lastControlMeshIterator);
+                }
 
-            newMeshPoint.origin = MeshPointOrigin::LastPoint();
-            newMeshPoint.type = MeshPointType::State;
-            newMeshPoint.time = endTime;
+                newMeshPoint.origin = MeshPointOrigin::LastPoint();
+                newMeshPoint.type = MeshPointType::State;
+                newMeshPoint.time = endTime;
 
-            if (lastControlMeshIterator->time < endTime){
-                if ((lastControlMeshIterator->time + m_pimpl->minStepSize) > endTime){ //if last control mesh point is too close to the end, remove it and place a state mesh point at the end
-                    m_pimpl->setIgnoredMesh(lastControlMeshIterator);
-                    controlMeshes--;
+                if (lastControlMeshIterator->time < endTime){
+                    if ((lastControlMeshIterator->time + m_minStepSize) > endTime){ //if last control mesh point is too close to the end, remove it and place a state mesh point at the end
+                        setIgnoredMesh(lastControlMeshIterator);
+                        controlMeshes--;
+                    }
+                    addMeshPoint(newMeshPoint);  //add a mesh point at the end;
                 }
-                m_pimpl->addMeshPoint(newMeshPoint);  //add a mesh point at the end;
+                return controlMeshes;
             }
-            return controlMeshes;
-        }
 
-        bool MultipleShootingTranscription::preliminaryChecks()
-        {
-            if (!(m_pimpl->ocproblem)){
-                reportError("MultipleShootingTranscription", "prepare",
-                            "Optimal control problem not set.");
-                return false;
-            }
+            bool preliminaryChecks() {
+                if (!(m_ocproblem)){
+                    reportError("MultipleShootingTranscription", "prepare",
+                                "Optimal control problem not set.");
+                    return false;
+                }
 
-            if (!(m_pimpl->integrator)) {
-                reportError("MultipleShootingTranscription", "prepare",
-                            "Integrator not set.");
-                return false;
-            }
+                if (!(m_integrator)) {
+                    reportError("MultipleShootingTranscription", "prepare",
+                                "Integrator not set.");
+                    return false;
+                }
 
-            if ((m_pimpl->ocproblem->finalTime() - m_pimpl->ocproblem->initialTime()) < m_pimpl->minStepSize){
-                reportError("MultipleShootingTranscription", "prepare",
-                            "The time horizon defined in the OptimalControlProblem is smaller than the minimum step size.");
-                return false;
-            }
+                if ((m_ocproblem->finalTime() - m_ocproblem->initialTime()) < m_minStepSize){
+                    reportError("MultipleShootingTranscription", "prepare",
+                                "The time horizon defined in the OptimalControlProblem is smaller than the minimum step size.");
+                    return false;
+                }
 
-            if (m_pimpl->controlPeriod < m_pimpl->minStepSize){
-                reportError("MultipleShootingTranscription", "prepare", "The control period cannot be lower than the minimum step size.");
-                return false;
-            }
+                if (m_controlPeriod < m_minStepSize){
+                    reportError("MultipleShootingTranscription", "prepare", "The control period cannot be lower than the minimum step size.");
+                    return false;
+                }
 
-            if ((m_pimpl->controlPeriod >= m_pimpl->maxStepSize) && (m_pimpl->controlPeriod <= (2 * m_pimpl->minStepSize))){
-                reportError("MultipleShootingTranscription", "prepare",
-                            "Cannot add control mesh points when the controller period is in the range [dTMax, 2*dTmin]."); // the first mesh point is a control mesh. The second mesh than would be too far from the first but not far enough to put a state mesh point in the middle.
-                return false;
-            }
+                if ((m_controlPeriod >= m_maxStepSize) && (m_controlPeriod <= (2 * m_minStepSize))){
+                    reportError("MultipleShootingTranscription", "prepare",
+                                "Cannot add control mesh points when the controller period is in the range [dTMax, 2*dTmin]."); // the first mesh point is a control mesh. The second mesh than would be too far from the first but not far enough to put a state mesh point in the middle.
+                    return false;
+                }
 
-            if (!(m_pimpl->prepared)){
-                if (m_pimpl->ocproblem->dynamicalSystem().expired()){
-                    if (m_pimpl->integrator->dynamicalSystem().expired()){
-                        reportError("MultipleShootingTranscription", "prepare",
-                                    "No dynamical system set, neither to the OptimalControlProblem nor to the Integrator object.");
-                        return false;
-                    }
-                    if (!(m_pimpl->ocproblem->setDynamicalSystemConstraint(m_pimpl->integrator->dynamicalSystem().lock()))){
-                        reportError("MultipleShootingTranscription", "prepare",
-                                    "Error while setting the dynamicalSystem to the OptimalControlProblem using the one pointer provided by the Integrator object.");
-                        return false;
-                    }
-                } else {
-                    if (!(m_pimpl->integrator->dynamicalSystem().expired()) &&
-                            (m_pimpl->integrator->dynamicalSystem().lock() != m_pimpl->ocproblem->dynamicalSystem().lock())){
-                        reportError("MultipleShootingTranscription", "prepare",
-                                    "The selected OptimalControlProblem and the Integrator point to two different dynamical systems.");
-                        return false;
-                    } else if (m_pimpl->integrator->dynamicalSystem().expired()) {
-                        if (!(m_pimpl->integrator->setDynamicalSystem(m_pimpl->ocproblem->dynamicalSystem().lock()))){
+                if (!(m_prepared)){
+                    if (m_ocproblem->dynamicalSystem().expired()){
+                        if (m_integrator->dynamicalSystem().expired()){
                             reportError("MultipleShootingTranscription", "prepare",
-                                        "Error while setting the dynamicalSystem to the Integrator using the one pointer provided by the OptimalControlProblem object.");
+                                        "No dynamical system set, neither to the OptimalControlProblem nor to the Integrator object.");
                             return false;
                         }
+                        if (!(m_ocproblem->setDynamicalSystemConstraint(m_integrator->dynamicalSystem().lock()))){
+                            reportError("MultipleShootingTranscription", "prepare",
+                                        "Error while setting the dynamicalSystem to the OptimalControlProblem using the one pointer provided by the Integrator object.");
+                            return false;
+                        }
+                    } else {
+                        if (!(m_integrator->dynamicalSystem().expired()) &&
+                                (m_integrator->dynamicalSystem().lock() != m_ocproblem->dynamicalSystem().lock())){
+                            reportError("MultipleShootingTranscription", "prepare",
+                                        "The selected OptimalControlProblem and the Integrator point to two different dynamical systems.");
+                            return false;
+                        } else if (m_integrator->dynamicalSystem().expired()) {
+                            if (!(m_integrator->setDynamicalSystem(m_ocproblem->dynamicalSystem().lock()))){
+                                reportError("MultipleShootingTranscription", "prepare",
+                                            "Error while setting the dynamicalSystem to the Integrator using the one pointer provided by the OptimalControlProblem object.");
+                                return false;
+                            }
+                        }
                     }
                 }
-            }
-
-            if (!(m_pimpl->integrator->setMaximumStepSize(m_pimpl->maxStepSize))){
-                reportError("MultipleShootingTranscription", "prepare","Error while setting the maximum step size to the integrator.");
-                return false;
-            }
-
-            return true;
-        }
-
 
-        bool MultipleShootingTranscription::setMeshPoints()
-        {
-            double endTime = m_pimpl->ocproblem->finalTime(), initTime = m_pimpl->ocproblem->initialTime();
-            m_pimpl->resetMeshPoints();
-
-            if (m_pimpl->controlPeriod < 2*(m_pimpl->minStepSize)){ //no other mesh point could be inserted otherwise
-                size_t controlMeshes = setControlMeshPoints();
-                size_t totalMeshes = static_cast<size_t>(m_pimpl->meshPointsEnd - m_pimpl->meshPoints.begin());
-
-                if (m_pimpl->prepared){
-                    if ((m_pimpl->totalMeshes != totalMeshes) || (m_pimpl->controlMeshes != controlMeshes)){
-                        reportWarning("MultipleShootingSolver", "setMeshPoints", "Unable to keep number of variables constant.");
-                    }
+                if (!(m_integrator->setMaximumStepSize(m_maxStepSize))){
+                    reportError("MultipleShootingTranscription", "prepare","Error while setting the maximum step size to the integrator.");
+                    return false;
                 }
 
-                m_pimpl->controlMeshes = controlMeshes;
-                m_pimpl->totalMeshes = totalMeshes;
                 return true;
             }
 
-            setControlMeshPoints();
-
-            MeshPoint newMeshPoint;
-            //Adding user defined control mesh points
-            newMeshPoint.origin = MeshPointOrigin::UserControl();
-            newMeshPoint.type = MeshPointType::Control;
-            for (size_t i = 0; i < m_pimpl->userControlMeshes.size(); ++i){
-               if ((m_pimpl->userControlMeshes[i] > initTime) && (m_pimpl->userControlMeshes[i] < endTime)){
-                    newMeshPoint.time = m_pimpl->userControlMeshes[i];
-                    m_pimpl->addMeshPoint(newMeshPoint);
-               } else {
-                   std::ostringstream errorMsg;
-                   errorMsg << "Ignored user defined control mesh point at time " << m_pimpl->userControlMeshes[i] << "Out of time range.";
-                   reportWarning("MultipleShootingSolver", "setMeshPoints", errorMsg.str().c_str());
-               }
-            }
+            bool setMeshPoints() {
+                double endTime = m_ocproblem->finalTime(), initTime = m_ocproblem->initialTime();
+                resetMeshPoints();
 
-            //Adding user defined state mesh points
-            newMeshPoint.origin = MeshPointOrigin::UserState();
-            newMeshPoint.type = MeshPointType::State;
-            for (size_t i = 0; i < m_pimpl->userStateMeshes.size(); ++i){
-                if ((m_pimpl->userStateMeshes[i] > initTime) && (m_pimpl->userStateMeshes[i] < endTime)){
-                    newMeshPoint.time = m_pimpl->userStateMeshes[i];
-                    m_pimpl->addMeshPoint(newMeshPoint);
-                } else {
-                    std::ostringstream errorMsg;
-                    errorMsg << "Ignored user defined state mesh point at time " << m_pimpl->userStateMeshes[i] << "Out of time range.";
-                    reportWarning("MultipleShootingSolver", "setMeshPoints", errorMsg.str().c_str());
-                }
-            }
+                if (m_controlPeriod < 2*(m_minStepSize)){ //no other mesh point could be inserted otherwise
+                    size_t newControlMeshes = setControlMeshPoints();
+                    size_t newTotalMeshes = static_cast<size_t>(m_meshPointsEnd - m_meshPoints.begin());
 
-            std::vector<TimeRange> &constraintsTRs = m_pimpl->ocproblem->getConstraintsTimeRanges();
-            std::vector<TimeRange> &costsTRs = m_pimpl->ocproblem->getCostsTimeRanges();
-
-            //adding mesh points for the constraint time ranges
-            newMeshPoint.type = MeshPointType::State;
-            for (size_t i = 0; i < constraintsTRs.size(); ++i){
-                if (constraintsTRs[i].isInstant()) {
-                    if (!(constraintsTRs[i].isInRange(initTime)) && !(constraintsTRs[i].isInRange(endTime)) && (constraintsTRs[i].initTime() > initTime)){ //we will have a mesh there in any case
-                        newMeshPoint.origin = MeshPointOrigin::Instant();
-                        newMeshPoint.time = constraintsTRs[i].initTime();
-                        m_pimpl->addMeshPoint(newMeshPoint);
-                    }
-                }
-                else {
-                    newMeshPoint.origin = MeshPointOrigin::TimeRange();
-                    if ((constraintsTRs[i].initTime() > initTime) && (constraintsTRs[i].initTime() < endTime)){
-                        newMeshPoint.time = constraintsTRs[i].initTime();
-                        m_pimpl->addMeshPoint(newMeshPoint);
+                    if (m_prepared){
+                        if ((m_totalMeshes != newTotalMeshes) || (m_controlMeshes != newControlMeshes)){
+                            reportWarning("MultipleShootingSolver", "setMeshPoints", "Unable to keep number of variables constant.");
+                        }
                     }
-                    if ((constraintsTRs[i].endTime() > initTime) && (constraintsTRs[i].endTime() < endTime)){
-                        newMeshPoint.time = constraintsTRs[i].endTime();
-                        m_pimpl->addMeshPoint(newMeshPoint);
+
+                    m_controlMeshes = newControlMeshes;
+                    m_totalMeshes = newTotalMeshes;
+                    return true;
+                }
+
+                setControlMeshPoints();
+
+                MeshPoint newMeshPoint;
+                //Adding user defined control mesh points
+                newMeshPoint.origin = MeshPointOrigin::UserControl();
+                newMeshPoint.type = MeshPointType::Control;
+                for (size_t i = 0; i < m_userControlMeshes.size(); ++i){
+                   if ((m_userControlMeshes[i] > initTime) && (m_userControlMeshes[i] < endTime)){
+                        newMeshPoint.time = m_userControlMeshes[i];
+                        addMeshPoint(newMeshPoint);
+                   } else {
+                       std::ostringstream errorMsg;
+                       errorMsg << "Ignored user defined control mesh point at time " << m_userControlMeshes[i] << "Out of time range.";
+                       reportWarning("MultipleShootingSolver", "setMeshPoints", errorMsg.str().c_str());
+                   }
+                }
+
+                //Adding user defined state mesh points
+                newMeshPoint.origin = MeshPointOrigin::UserState();
+                newMeshPoint.type = MeshPointType::State;
+                for (size_t i = 0; i < m_userStateMeshes.size(); ++i){
+                    if ((m_userStateMeshes[i] > initTime) && (m_userStateMeshes[i] < endTime)){
+                        newMeshPoint.time = m_userStateMeshes[i];
+                        addMeshPoint(newMeshPoint);
+                    } else {
+                        std::ostringstream errorMsg;
+                        errorMsg << "Ignored user defined state mesh point at time " << m_userStateMeshes[i] << "Out of time range.";
+                        reportWarning("MultipleShootingSolver", "setMeshPoints", errorMsg.str().c_str());
                     }
                 }
-            }
 
+                std::vector<TimeRange> &constraintsTRs = m_ocproblem->getConstraintsTimeRanges();
+                std::vector<TimeRange> &costsTRs = m_ocproblem->getCostsTimeRanges();
 
-            //adding mesh points for the costs time ranges
-            for (size_t i = 0; i < costsTRs.size(); ++i){
-                if (costsTRs[i].isInstant()){
-                    if (!(costsTRs[i].isInRange(initTime)) && !(costsTRs[i].isInRange(endTime)) && (costsTRs[i].initTime() > initTime)) { //we will have a mesh there in any case
-                        newMeshPoint.origin = MeshPointOrigin::Instant();
-                        newMeshPoint.time = costsTRs[i].initTime();
-                        m_pimpl->addMeshPoint(newMeshPoint);
+                //adding mesh points for the constraint time ranges
+                newMeshPoint.type = MeshPointType::State;
+                for (size_t i = 0; i < constraintsTRs.size(); ++i){
+                    if (constraintsTRs[i].isInstant()) {
+                        if (!(constraintsTRs[i].isInRange(initTime)) && !(constraintsTRs[i].isInRange(endTime)) && (constraintsTRs[i].initTime() > initTime)){ //we will have a mesh there in any case
+                            newMeshPoint.origin = MeshPointOrigin::Instant();
+                            newMeshPoint.time = constraintsTRs[i].initTime();
+                            addMeshPoint(newMeshPoint);
+                        }
+                    }
+                    else {
+                        newMeshPoint.origin = MeshPointOrigin::TimeRange();
+                        if ((constraintsTRs[i].initTime() > initTime) && (constraintsTRs[i].initTime() < endTime)){
+                            newMeshPoint.time = constraintsTRs[i].initTime();
+                            addMeshPoint(newMeshPoint);
+                        }
+                        if ((constraintsTRs[i].endTime() > initTime) && (constraintsTRs[i].endTime() < endTime)){
+                            newMeshPoint.time = constraintsTRs[i].endTime();
+                            addMeshPoint(newMeshPoint);
+                        }
                     }
                 }
-                else {
-                    newMeshPoint.origin = MeshPointOrigin::TimeRange();
-                    if ((costsTRs[i].initTime() > initTime) && (costsTRs[i].initTime() < endTime)){
-                        newMeshPoint.time = costsTRs[i].initTime();
-                        m_pimpl->addMeshPoint(newMeshPoint);
+
+
+                //adding mesh points for the costs time ranges
+                for (size_t i = 0; i < costsTRs.size(); ++i){
+                    if (costsTRs[i].isInstant()){
+                        if (!(costsTRs[i].isInRange(initTime)) && !(costsTRs[i].isInRange(endTime)) && (costsTRs[i].initTime() > initTime)) { //we will have a mesh there in any case
+                            newMeshPoint.origin = MeshPointOrigin::Instant();
+                            newMeshPoint.time = costsTRs[i].initTime();
+                            addMeshPoint(newMeshPoint);
+                        }
                     }
-                    if ((costsTRs[i].endTime() > initTime) && (costsTRs[i].endTime() < endTime)){
-                        newMeshPoint.time = costsTRs[i].endTime();
-                        m_pimpl->addMeshPoint(newMeshPoint);
+                    else {
+                        newMeshPoint.origin = MeshPointOrigin::TimeRange();
+                        if ((costsTRs[i].initTime() > initTime) && (costsTRs[i].initTime() < endTime)){
+                            newMeshPoint.time = costsTRs[i].initTime();
+                            addMeshPoint(newMeshPoint);
+                        }
+                        if ((costsTRs[i].endTime() > initTime) && (costsTRs[i].endTime() < endTime)){
+                            newMeshPoint.time = costsTRs[i].endTime();
+                            addMeshPoint(newMeshPoint);
+                        }
                     }
                 }
-            }
 
-            m_pimpl->cleanLeftoverMeshes(); //set to ignored the leftover meshes, i.e. those that were set in a previous call
+                cleanLeftoverMeshes(); //set to ignored the leftover meshes, i.e. those that were set in a previous call
 
-            double tMin = m_pimpl->minStepSize;
-            std::sort(m_pimpl->meshPoints.begin(), m_pimpl->meshPointsEnd,
-                      [tMin](const MeshPoint&a, const MeshPoint&b) {
-                            if (std::abs(b.time - a.time) < tMin)
-                                return a.origin.priority() < b.origin.priority();
-                            else return a.time < b.time;}); //reorder the vector. Using this ordering, by scrolling the vector, in case of two narrow points, I will get the more important first
+                double tMin = m_minStepSize;
+                std::sort(m_meshPoints.begin(), m_meshPointsEnd,
+                          [tMin](const MeshPoint&a, const MeshPoint&b) {
+                                if (std::abs(b.time - a.time) < tMin)
+                                    return a.origin.priority() < b.origin.priority();
+                                else return a.time < b.time;}); //reorder the vector. Using this ordering, by scrolling the vector, in case of two narrow points, I will get the more important first
 
-            //Now I need to prune the vector
+                //Now I need to prune the vector
 
-            std::vector<MeshPoint>::iterator mesh = m_pimpl->meshPoints.begin();
-            std::vector<MeshPoint>::iterator nextMesh = mesh;
-            MeshPointOrigin last = MeshPointOrigin::LastPoint();
-            MeshPointOrigin ignored = MeshPointOrigin::Ignored();
-            MeshPointOrigin timeRangeOrigin = MeshPointOrigin::TimeRange();
+                std::vector<MeshPoint>::iterator mesh = m_meshPoints.begin();
+                std::vector<MeshPoint>::iterator nextMesh = mesh;
+                MeshPointOrigin last = MeshPointOrigin::LastPoint();
+                MeshPointOrigin ignored = MeshPointOrigin::Ignored();
+                MeshPointOrigin timeRangeOrigin = MeshPointOrigin::TimeRange();
 
-            newMeshPoint.origin = MeshPointOrigin::CompensateLongPeriod();
-            newMeshPoint.type = MeshPointType::State;
+                newMeshPoint.origin = MeshPointOrigin::CompensateLongPeriod();
+                newMeshPoint.type = MeshPointType::State;
 
-            double timeDistance;
-            while (mesh->origin != last){
+                double timeDistance;
+                while (mesh->origin != last){
 
-                nextMesh = m_pimpl->findNextMeshPoint(mesh); //find next valid mesh
-                timeDistance = std::abs(nextMesh->time - mesh->time); //for the way I have ordered the vector, it can be negative
+                    nextMesh = findNextMeshPoint(mesh); //find next valid mesh
+                    timeDistance = std::abs(nextMesh->time - mesh->time); //for the way I have ordered the vector, it can be negative
 
-                if (timeDistance > m_pimpl->maxStepSize){ //two consecutive points are too distant
-                    unsigned int additionalPoints = static_cast<unsigned int>(std::ceil(timeDistance/(m_pimpl->maxStepSize))) - 1;
-                    double dtAdd = timeDistance / (additionalPoints + 1); //additionalPoints + 1 is the number of segments.
-                    long nextPosition = nextMesh - m_pimpl->meshPoints.begin();
-                    //since tmin < tmax/2, dtAdd > tmin. Infact, the worst case is when timeDistance is nearly equal to tmax.
-                    for (unsigned int i = 0; i < additionalPoints; ++i) {
-                        newMeshPoint.time = mesh->time + (i + 1)*dtAdd;
-                        m_pimpl->addMeshPoint(newMeshPoint);
-                    }
-                    nextMesh = m_pimpl->meshPoints.begin() + nextPosition;
-                    mesh = nextMesh;
-
-                } else if (timeDistance < m_pimpl->minStepSize){ //too consecutive points are too close
-
-                    if (nextMesh->origin < mesh->origin){ //check who has the higher priority
-                        m_pimpl->priorityWarning(nextMesh, timeRangeOrigin);
-                        m_pimpl->setIgnoredMesh(nextMesh);
-                    } else { //actually, because of the ordering, this case should never be reached.
-                        m_pimpl->priorityWarning(mesh, timeRangeOrigin);
-                        m_pimpl->setIgnoredMesh(mesh);
-                        do {
-                            --mesh;
-                        } while (mesh->origin == ignored);
-                        nextMesh = mesh;
+                    if (timeDistance > m_maxStepSize){ //two consecutive points are too distant
+                        unsigned int additionalPoints = static_cast<unsigned int>(std::ceil(timeDistance/(m_maxStepSize))) - 1;
+                        double dtAdd = timeDistance / (additionalPoints + 1); //additionalPoints + 1 is the number of segments.
+                        long nextPosition = nextMesh - m_meshPoints.begin();
+                        //since tmin < tmax/2, dtAdd > tmin. Infact, the worst case is when timeDistance is nearly equal to tmax.
+                        for (unsigned int i = 0; i < additionalPoints; ++i) {
+                            newMeshPoint.time = mesh->time + (i + 1)*dtAdd;
+                            addMeshPoint(newMeshPoint);
+                        }
+                        nextMesh = m_meshPoints.begin() + nextPosition;
+                        mesh = nextMesh;
+
+                    } else if (timeDistance < m_minStepSize){ //too consecutive points are too close
+
+                        if (nextMesh->origin < mesh->origin){ //check who has the higher priority
+                            priorityWarning(nextMesh, timeRangeOrigin);
+                            setIgnoredMesh(nextMesh);
+                        } else { //actually, because of the ordering, this case should never be reached.
+                            priorityWarning(mesh, timeRangeOrigin);
+                            setIgnoredMesh(mesh);
+                            do {
+                                --mesh;
+                            } while (mesh->origin == ignored);
+                            nextMesh = mesh;
+                        }
+                    } else {
+                        mesh = nextMesh;
                     }
-                } else {
-                    mesh = nextMesh;
-                }
-            }
-
-            std::sort(m_pimpl->meshPoints.begin(), m_pimpl->meshPointsEnd,
-                      [](const MeshPoint&a, const MeshPoint&b) {return a.time < b.time;}); //reorder the vector
-
-            size_t totalMeshes = 0, controlMeshes = 0;
-            mesh = m_pimpl->meshPoints.begin();
-
-            while (mesh->origin != last){ //count meshes
-                if (mesh->type == MeshPointType::Control){
-                    controlMeshes++;
                 }
-                totalMeshes++;
-                ++mesh;
-            }
-            totalMeshes++; //the last mesh
-            m_pimpl->meshPointsEnd = m_pimpl->meshPoints.begin() + static_cast<long>(totalMeshes);
-            assert((m_pimpl->meshPointsEnd - 1)->origin == last);
-
-            if (m_pimpl->prepared){
-                if (m_pimpl->controlMeshes == controlMeshes){
 
-                    if (m_pimpl->totalMeshes != totalMeshes){
+                std::sort(m_meshPoints.begin(), m_meshPointsEnd,
+                          [](const MeshPoint&a, const MeshPoint&b) {return a.time < b.time;}); //reorder the vector
 
-                        if (m_pimpl->totalMeshes < totalMeshes){ //here I need to remove meshes
+                size_t newTotalMeshes = 0, newControlMeshes = 0;
+                mesh = m_meshPoints.begin();
 
-                            size_t toBeRemoved = totalMeshes - m_pimpl->totalMeshes;
-                            mesh = m_pimpl->meshPoints.begin();
-                            nextMesh = mesh;
-                            std::vector<MeshPoint>::iterator nextNextMesh = mesh;
-                            while ((mesh->origin != last) && (toBeRemoved > 0)){
-                                nextMesh = m_pimpl->findNextMeshPoint(mesh); //find next valid mesh
-
-                                if (nextMesh->origin != last) {
-                                    nextNextMesh = m_pimpl->findNextMeshPoint(nextMesh);
-                                    timeDistance = std::abs(nextNextMesh->time - mesh->time);
-
-                                    if ((timeDistance < m_pimpl->maxStepSize) && !(timeRangeOrigin < nextMesh->origin)){ //I can remove nextMesh
-                                        m_pimpl->setIgnoredMesh(nextMesh);
-                                        toBeRemoved--;
-                                        totalMeshes--;
+                while (mesh->origin != last){ //count meshes
+                    if (mesh->type == MeshPointType::Control){
+                        newControlMeshes++;
+                    }
+                    newTotalMeshes++;
+                    ++mesh;
+                }
+                newTotalMeshes++; //the last mesh
+                m_meshPointsEnd = m_meshPoints.begin() + static_cast<long>(newTotalMeshes);
+                assert((m_meshPointsEnd - 1)->origin == last);
+
+                if (m_prepared){
+                    if (m_controlMeshes == newControlMeshes){
+
+                        if (m_totalMeshes != newTotalMeshes){
+
+                            if (m_totalMeshes < newTotalMeshes){ //here I need to remove meshes
+
+                                size_t toBeRemoved = newTotalMeshes - m_totalMeshes;
+                                mesh = m_meshPoints.begin();
+                                nextMesh = mesh;
+                                std::vector<MeshPoint>::iterator nextNextMesh = mesh;
+                                while ((mesh->origin != last) && (toBeRemoved > 0)){
+                                    nextMesh = findNextMeshPoint(mesh); //find next valid mesh
+
+                                    if (nextMesh->origin != last) {
+                                        nextNextMesh = findNextMeshPoint(nextMesh);
+                                        timeDistance = std::abs(nextNextMesh->time - mesh->time);
+
+                                        if ((timeDistance < m_maxStepSize) && !(timeRangeOrigin < nextMesh->origin)){ //I can remove nextMesh
+                                            setIgnoredMesh(nextMesh);
+                                            toBeRemoved--;
+                                            newTotalMeshes--;
+                                        } else {
+                                            mesh = nextMesh;
+                                        }
                                     } else {
                                         mesh = nextMesh;
                                     }
-                                } else {
-                                    mesh = nextMesh;
                                 }
-                            }
-                            if (toBeRemoved > 0){
-                                reportWarning("MultipleShootingSolver", "setMeshPoints",
-                                              "Unable to keep a constant number of variables. Unable to remove enough mesh points.");
-                            }
+                                if (toBeRemoved > 0){
+                                    reportWarning("MultipleShootingSolver", "setMeshPoints",
+                                                  "Unable to keep a constant number of variables. Unable to remove enough mesh points.");
+                                }
 
-                        } else if (m_pimpl->totalMeshes > totalMeshes){ //here I need to add meshes
-                            unsigned int toBeAdded = static_cast<unsigned int>(m_pimpl->totalMeshes - totalMeshes);
-                            mesh = m_pimpl->meshPoints.begin();
-                            nextMesh = mesh;
-                            newMeshPoint.origin = MeshPointOrigin::FillVariables();
-                            newMeshPoint.type = MeshPointType::State;
-                            while ((mesh->origin != last) && (toBeAdded > 0)){
-                                nextMesh = m_pimpl->findNextMeshPoint(mesh); //find next valid mesh
-                                timeDistance = std::abs(nextMesh->time - mesh->time);
-
-                                if (timeDistance > (2*m_pimpl->minStepSize)){
-                                    unsigned int possibleMeshes = static_cast<unsigned int>(std::ceil(timeDistance/(m_pimpl->minStepSize))) - 1;
-                                    unsigned int meshToAddHere = std::min(toBeAdded, possibleMeshes);
-                                    double dT = timeDistance/(meshToAddHere + 1);
-                                    long nextPosition = nextMesh - m_pimpl->meshPoints.begin();
-                                    for(unsigned int m = 1; m <= meshToAddHere; m++){
-                                        newMeshPoint.time = mesh->time + m*dT;
-                                        m_pimpl->addMeshPoint(newMeshPoint);
-                                        totalMeshes++;
+                            } else if (m_totalMeshes > newTotalMeshes){ //here I need to add meshes
+                                unsigned int toBeAdded = static_cast<unsigned int>(m_totalMeshes - newTotalMeshes);
+                                mesh = m_meshPoints.begin();
+                                nextMesh = mesh;
+                                newMeshPoint.origin = MeshPointOrigin::FillVariables();
+                                newMeshPoint.type = MeshPointType::State;
+                                while ((mesh->origin != last) && (toBeAdded > 0)){
+                                    nextMesh = findNextMeshPoint(mesh); //find next valid mesh
+                                    timeDistance = std::abs(nextMesh->time - mesh->time);
+
+                                    if (timeDistance > (2*m_minStepSize)){
+                                        unsigned int possibleMeshes = static_cast<unsigned int>(std::ceil(timeDistance/(m_minStepSize))) - 1;
+                                        unsigned int meshToAddHere = std::min(toBeAdded, possibleMeshes);
+                                        double dT = timeDistance/(meshToAddHere + 1);
+                                        long nextPosition = nextMesh - m_meshPoints.begin();
+                                        for(unsigned int m = 1; m <= meshToAddHere; m++){
+                                            newMeshPoint.time = mesh->time + m*dT;
+                                            addMeshPoint(newMeshPoint);
+                                            newTotalMeshes++;
+                                        }
+                                        nextMesh = m_meshPoints.begin() + nextPosition;
                                     }
-                                    nextMesh = m_pimpl->meshPoints.begin() + nextPosition;
+                                    mesh = nextMesh;
                                 }
-                                mesh = nextMesh;
-                            }
 
-                            if (toBeAdded > 0){
-                                reportWarning("MultipleShootingSolver", "setMeshPoints",
-                                              "Unable to keep a constant number of variables. Unable to add enough mesh points.");
-                            }
+                                if (toBeAdded > 0){
+                                    reportWarning("MultipleShootingSolver", "setMeshPoints",
+                                                  "Unable to keep a constant number of variables. Unable to add enough mesh points.");
+                                }
 
+                            }
+                            std::sort(m_meshPoints.begin(), m_meshPointsEnd,
+                                      [](const MeshPoint&a, const MeshPoint&b) {return a.time < b.time;}); //reorder the vector
+                            m_meshPointsEnd = m_meshPoints.begin() + static_cast<long>(newTotalMeshes);
+                            assert((m_meshPointsEnd - 1)->origin == last);
                         }
-                        std::sort(m_pimpl->meshPoints.begin(), m_pimpl->meshPointsEnd,
-                                  [](const MeshPoint&a, const MeshPoint&b) {return a.time < b.time;}); //reorder the vector
-                        m_pimpl->meshPointsEnd = m_pimpl->meshPoints.begin() + static_cast<long>(totalMeshes);
-                        assert((m_pimpl->meshPointsEnd - 1)->origin == last);
+                    } else {
+                        reportWarning("MultipleShootingSolver", "setMeshPoints",
+                                      "Unable to keep a constant number of variables. The control meshes are different");
                     }
-                } else {
-                    reportWarning("MultipleShootingSolver", "setMeshPoints",
-                                  "Unable to keep a constant number of variables. The control meshes are different");
                 }
-            }
 
-            m_pimpl->totalMeshes = totalMeshes;
-            m_pimpl->controlMeshes = controlMeshes;
+                m_totalMeshes = newTotalMeshes;
+                m_controlMeshes = newControlMeshes;
 
-            return true;
-        }
-
-        MultipleShootingTranscription::~MultipleShootingTranscription()
-        {
-            if (m_pimpl){
-                delete m_pimpl;
-                m_pimpl = nullptr;
+                return true;
             }
-        }
 
-        bool MultipleShootingTranscription::setOptimalControlProblem(const std::shared_ptr<OptimalControlProblem> problem)
-        {
-            if (m_pimpl->ocproblem){
-                reportError("MultipleShootingSolver", "setOptimalControlProblem", "The OptimalControlProblem for this instance has already been set.");
-                return false;
-            }
+            bool setOptimalControlProblem(const std::shared_ptr<OptimalControlProblem> problem) {
+                if (!problem) {
+                    reportError("MultipleShootingSolver", "setOptimalControlProblem", "Empty pointer.");
+                    return false;
+                }
 
-            m_pimpl->ocproblem = problem;
-            return true;
-        }
+                if (m_ocproblem){
+                    reportError("MultipleShootingSolver", "setOptimalControlProblem", "The OptimalControlProblem for this instance has already been set.");
+                    return false;
+                }
 
-        bool MultipleShootingTranscription::setIntegrator(const std::shared_ptr<Integrator> integrationMethod)
-        {
-            if (m_pimpl->integrator){
-                reportError("MultipleShootingSolver", "setIntegrator", "The integration method for this instance has already been set.");
-                return false;
+                m_ocproblem = problem;
+                return true;
             }
 
-            m_pimpl->integrator = integrationMethod;
-            return true;
-        }
+            bool setIntegrator(const std::shared_ptr<Integrator> integrationMethod) {
+                if (!(integrationMethod)) {
+                    reportError("MultipleShootingSolver", "setIntegrator", "Empty pointer.");
+                    return false;
+                }
 
-        bool MultipleShootingTranscription::setStepSizeBounds(const double minStepSize, const double maxStepsize)
-        {
-            if (minStepSize <= 0){
-                reportError("MultipleShootingSolver", "setStepSizeBounds","The minimum step size is expected to be positive.");
-                return false;
-            }
+                if (m_integrator){
+                    reportError("MultipleShootingSolver", "setIntegrator", "The integration method for this instance has already been set.");
+                    return false;
+                }
 
-            if (maxStepsize <= (2 * minStepSize)){
-                reportError("MultipleShootingSolver", "setStepSizeBounds","The maximum step size is expected to be greater than twice the minimum."); //imagine to have a distance between two mesh points slightly greater than the max. It would be impossible to add a mesh point in the middle
-                return false;
+                m_integrator = integrationMethod;
+                return true;
             }
 
-            m_pimpl->minStepSize = minStepSize;
-            m_pimpl->maxStepSize = maxStepsize;
-
-            return true;
-        }
-
-        bool MultipleShootingTranscription::setControlPeriod(double period)
-        {
-            if (period <= 0){
-                reportError("MultipleShootingSolver", "prepare", "The control period is supposed to be positive.");
-                return false;
-            }
-            m_pimpl->controlPeriod = period;
-            return true;
-        }
+            bool setStepSizeBounds(const double minStepSize, const double maxStepsize) {
+                if (minStepSize <= 0){
+                    reportError("MultipleShootingSolver", "setStepSizeBounds","The minimum step size is expected to be positive.");
+                    return false;
+                }
 
-        bool MultipleShootingTranscription::setAdditionalStateMeshPoints(const std::vector<double> &stateMeshes)
-        {
-            m_pimpl->userStateMeshes = stateMeshes;
-            return true;
-        }
+                if (maxStepsize <= (2 * minStepSize)){
+                    reportError("MultipleShootingSolver", "setStepSizeBounds","The maximum step size is expected to be greater than twice the minimum."); //imagine to have a distance between two mesh points slightly greater than the max. It would be impossible to add a mesh point in the middle
+                    return false;
+                }
 
-        bool MultipleShootingTranscription::setAdditionalControlMeshPoints(const std::vector<double> &controlMeshes)
-        {
-            m_pimpl->userControlMeshes = controlMeshes;
-            return true;
-        }
+                m_minStepSize = minStepSize;
+                m_maxStepSize = maxStepsize;
 
-        void MultipleShootingTranscription::setPlusInfinity(double plusInfinity)
-        {
-            assert(plusInfinity > 0);
-            m_pimpl->plusInfinity = plusInfinity;
-        }
+                return true;
+            }
 
-        void MultipleShootingTranscription::setMinusInfinity(double minusInfinity)
-        {
-            assert(minusInfinity < 0);
-            m_pimpl->minusInfinity = minusInfinity;
-        }
+            bool setControlPeriod(double period) {
+                if (period <= 0){
+                    reportError("MultipleShootingSolver", "prepare", "The control period is supposed to be positive.");
+                    return false;
+                }
+                m_controlPeriod = period;
+                return true;
+            }
 
-        bool MultipleShootingTranscription::setInitialState(const VectorDynSize &initialState)
-        {
-            if (!(m_pimpl->ocproblem)){
-                reportError("MultipleShootingTranscription", "setInitialState", "The optimal control problem pointer is empty.");
-                return false;
+            bool setAdditionalStateMeshPoints(const std::vector<double>& stateMeshes) {
+                m_userStateMeshes = stateMeshes;
+                return true;
             }
 
-            if (m_pimpl->ocproblem->dynamicalSystem().expired()){
-                reportError("MultipleShootingTranscription", "setInitialState", "The optimal control problem does not point to any dynamical system.");
-                return false;
+            bool setAdditionalControlMeshPoints(const std::vector<double>& controlMeshes) {
+                m_userControlMeshes = controlMeshes;
+                return true;
             }
 
-            if (!(m_pimpl->ocproblem->dynamicalSystem().lock()->setInitialState(initialState))){
-                reportError("MultipleShootingTranscription", "setInitialState", "Error while setting the initial state to the dynamical system.");
-                return false;
+            void setPlusInfinity(double plusInfinity) {
+                assert(plusInfinity > 0);
+                m_plusInfinity = plusInfinity;
             }
 
-            return true;
-        }
+            void setMinusInfinity(double minusInfinity) {
+                assert(minusInfinity < 0);
+                m_minusInfinity = minusInfinity;
+            }
 
-        bool MultipleShootingTranscription::getTimings(std::vector<double> &stateEvaluations, std::vector<double> &controlEvaluations)
-        {
-            if (!(m_pimpl->prepared)){
-                reportWarning("MultipleShootingTranscription", "getTimings", "The method solve was not called yet. Computing new mesh points. These may be overwritten when calling the solve method.");
+            bool setInitialState(const VectorDynSize &initialState) {
+                if (!(m_ocproblem)){
+                    reportError("MultipleShootingTranscription", "setInitialState", "The optimal control problem pointer is empty.");
+                    return false;
+                }
 
-                if (!preliminaryChecks()) {
+                if (m_ocproblem->dynamicalSystem().expired()){
+                    reportError("MultipleShootingTranscription", "setInitialState", "The optimal control problem does not point to any dynamical system.");
                     return false;
                 }
 
-                if (!setMeshPoints()){
+                if (!(m_ocproblem->dynamicalSystem().lock()->setInitialState(initialState))){
+                    reportError("MultipleShootingTranscription", "setInitialState", "Error while setting the initial state to the dynamical system.");
                     return false;
                 }
-            }
 
-            if (stateEvaluations.size() != (m_pimpl->totalMeshes - 1)) {
-                stateEvaluations.resize(m_pimpl->totalMeshes - 1);
+                return true;
             }
 
-            if (controlEvaluations.size() != m_pimpl->controlMeshes) {
-                controlEvaluations.resize(m_pimpl->controlMeshes);
-            }
+            bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations) {
+                if (!(m_prepared)){
+                    reportWarning("MultipleShootingTranscription", "getTimings", "The method solve was not called yet. Computing new mesh points. These may be overwritten when calling the solve method.");
 
-            size_t stateIndex = 0, controlIndex = 0;
+                    if (!preliminaryChecks()) {
+                        return false;
+                    }
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                    if (!setMeshPoints()){
+                        return false;
+                    }
+                }
 
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin != first){
-                    stateEvaluations[stateIndex] = mesh->time;
-                    stateIndex++;
+                if (stateEvaluations.size() != (m_totalMeshes - 1)) {
+                    stateEvaluations.resize(m_totalMeshes - 1);
                 }
-                if (mesh->type == MeshPointType::Control){
-                    controlEvaluations[controlIndex] = mesh->time;
-                    controlIndex++;
+
+                if (controlEvaluations.size() != m_controlMeshes) {
+                    controlEvaluations.resize(m_controlMeshes);
                 }
-            }
 
-            return true;
-        }
+                size_t stateIndex = 0, controlIndex = 0;
 
-        bool MultipleShootingTranscription::getSolution(std::vector<VectorDynSize> &states, std::vector<VectorDynSize> &controls)
-        {
-            if (!(m_pimpl->solved)){
-                reportError("MultipleShootingTranscription", "getSolution", "First you need to solve the problem once.");
-                return false;
-            }
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
 
-            if (states.size() != (m_pimpl->totalMeshes - 1)) {
-                states.resize((m_pimpl->totalMeshes - 1));
-            }
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin != first){
+                        stateEvaluations[stateIndex] = mesh->time;
+                        stateIndex++;
+                    }
+                    if (mesh->type == MeshPointType::Control){
+                        controlEvaluations[controlIndex] = mesh->time;
+                        controlIndex++;
+                    }
+                }
 
-            if (controls.size() != m_pimpl->controlMeshes) {
-                controls.resize(m_pimpl->controlMeshes);
+                return true;
             }
 
-            Eigen::Map<Eigen::VectorXd> solutionMap = toEigen(m_pimpl->solution);
-
-            size_t stateIndex = 0, controlIndex = 0;
-
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin != first){
-                    if (states[stateIndex].size() !=  m_pimpl->nx) {
-                        states[stateIndex].resize(m_pimpl->nx);
-                    }
+            bool getSolution(std::vector<VectorDynSize>& states, std::vector<VectorDynSize>& controls) {
+                if (!(m_solved)){
+                    reportError("MultipleShootingTranscription", "getSolution", "First you need to solve the problem once.");
+                    return false;
+                }
 
-                    toEigen(states[stateIndex]) = solutionMap.segment(mesh->stateIndex, m_pimpl->nx);
-                    stateIndex++;
+                if (states.size() != (m_totalMeshes - 1)) {
+                    states.resize((m_totalMeshes - 1));
                 }
-                if (mesh->type == MeshPointType::Control){
-                    if (controls[controlIndex].size() !=  m_pimpl->nu) {
-                        controls[controlIndex].resize(m_pimpl->nu);
-                    }
 
-                    toEigen(controls[controlIndex]) = solutionMap.segment(mesh->controlIndex, m_pimpl->nu);
-                    controlIndex++;
+                if (controls.size() != m_controlMeshes) {
+                    controls.resize(m_controlMeshes);
                 }
-            }
 
-            return true;
-        }
+                Eigen::Map<Eigen::VectorXd> solutionMap = toEigen(m_solution);
 
+                size_t stateIndex = 0, controlIndex = 0;
 
-        bool MultipleShootingTranscription::prepare()
-        {
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
 
-            if (!preliminaryChecks()){
-                return false;
-            }
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin != first){
+                        if (states[stateIndex].size() !=  m_nx) {
+                            states[stateIndex].resize(m_nx);
+                        }
+
+                        toEigen(states[stateIndex]) = solutionMap.segment(mesh->stateIndex, m_nx);
+                        stateIndex++;
+                    }
+                    if (mesh->type == MeshPointType::Control){
+                        if (controls[controlIndex].size() !=  m_nu) {
+                            controls[controlIndex].resize(m_nu);
+                        }
 
+                        toEigen(controls[controlIndex]) = solutionMap.segment(mesh->controlIndex, m_nu);
+                        controlIndex++;
+                    }
+                }
 
-            if (!setMeshPoints()){
-                return false;
+                return true;
             }
 
-            size_t nx = m_pimpl->integrator->dynamicalSystem().lock()->stateSpaceSize();
-            m_pimpl->nx = nx;
+        public:
 
-            size_t nu = m_pimpl->integrator->dynamicalSystem().lock()->controlSpaceSize();
-            m_pimpl->nu = nu;
+            MultipleShootingTranscription()
+            : m_ocproblem(nullptr)
+            ,m_integrator(nullptr)
+            ,m_totalMeshes(0)
+            ,m_controlMeshes(0)
+            ,m_prepared(false)
+            ,m_meshPointsEnd(m_meshPoints.end())
+            ,m_minStepSize(0.001)
+            ,m_maxStepSize(0.01)
+            ,m_controlPeriod(0.01)
+            ,m_nx(0)
+            ,m_nu(0)
+            ,m_numberOfVariables(0)
+            ,m_constraintsPerInstant(0)
+            ,m_numberOfConstraints(0)
+            ,m_plusInfinity(1e19)
+            ,m_minusInfinity(-1e19)
+            ,m_solved(false)
+            { }
+
+            MultipleShootingTranscription(const std::shared_ptr<OptimalControlProblem> problem, const std::shared_ptr<Integrator> integrationMethod)
+            :m_ocproblem(problem)
+            ,m_integrator(integrationMethod)
+            ,m_totalMeshes(0)
+            ,m_controlMeshes(0)
+            ,m_prepared(false)
+            ,m_meshPointsEnd(m_meshPoints.end())
+            ,m_minStepSize(0.001)
+            ,m_maxStepSize(0.01)
+            ,m_controlPeriod(0.01)
+            ,m_nx(0)
+            ,m_nu(0)
+            ,m_numberOfVariables(0)
+            ,m_constraintsPerInstant(0)
+            ,m_numberOfConstraints(0)
+            ,m_plusInfinity(1e19)
+            ,m_minusInfinity(-1e19)
+            ,m_solved(false)
+            { }
+
+            MultipleShootingTranscription(const MultipleShootingTranscription& other) = delete;
+
+            virtual ~MultipleShootingTranscription() override
+            { }
+
+            virtual bool prepare() override {
+
+                if (!preliminaryChecks()){
+                    return false;
+                }
 
-            //TODO: I should consider also the possibility to have auxiliary variables in the integrator
-            m_pimpl->numberOfVariables = (m_pimpl->totalMeshes - 1) * nx + m_pimpl->controlMeshes * nu; //the -1 removes the initial state from the set of optimization varibales
+                if (!setMeshPoints()){
+                    return false;
+                }
 
-            m_pimpl->constraintsPerInstant = m_pimpl->ocproblem->getConstraintsDimension();
-            size_t nc = m_pimpl->constraintsPerInstant;
-            m_pimpl->numberOfConstraints = (m_pimpl->totalMeshes - 1) * nx + (m_pimpl->constraintsPerInstant) * (m_pimpl->totalMeshes); //dynamical constraints (removing the initial state) and normal constraints
+                size_t nx = m_integrator->dynamicalSystem().lock()->stateSpaceSize();
+                m_nx = nx;
 
-            m_pimpl->allocateBuffers();
+                size_t nu = m_integrator->dynamicalSystem().lock()->controlSpaceSize();
+                m_nu = nu;
 
-            Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(m_pimpl->constraintsLowerBound);
-            Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(m_pimpl->constraintsUpperBound);
+                //TODO: I should consider also the possibility to have auxiliary variables in the integrator
+                m_numberOfVariables = (m_totalMeshes - 1) * nx + m_controlMeshes * nu; //the -1 removes the initial state from the set of optimization varibales
+
+                m_constraintsPerInstant = m_ocproblem->getConstraintsDimension();
+                size_t nc = m_constraintsPerInstant;
+                m_numberOfConstraints = (m_totalMeshes - 1) * nx + (m_constraintsPerInstant) * (m_totalMeshes); //dynamical constraints (removing the initial state) and normal constraints
+
+                allocateBuffers();
+
+                Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(m_constraintsLowerBound);
+                Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(m_constraintsUpperBound);
+
+                resetNonZerosCount();
+
+                std::vector<MeshPoint>::iterator mesh = m_meshPoints.begin(), previousControlMesh = mesh;
+                size_t index = 0, constraintIndex = 0;
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                while (mesh != m_meshPointsEnd){
+                    if (mesh->origin == first){
+                        //setting up the indeces
+                        mesh->controlIndex = index;
+                        mesh->previousControlIndex = index;
+                        index += nu;
+                        previousControlMesh = mesh;
+
+                        //Saving constraints bounds
+                        if (!(m_ocproblem->getConstraintsLowerBound(mesh->time, m_minusInfinity, m_constraintsBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
+                            reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
+                        }
+                        lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
 
-            m_pimpl->resetNonZerosCount();
+                        if (!(m_ocproblem->getConstraintsUpperBound(mesh->time, m_plusInfinity, m_constraintsBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
+                            reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
+                        }
+                        upperBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+
+                        //Saving the jacobian structure due to the constraints
+                        addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        constraintIndex += nc;
+
+                        //Saving the hessian structure
+                        addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+
+                    } else if (mesh->type == MeshPointType::Control) {
+                        mesh->previousControlIndex = previousControlMesh->controlIndex;
+                        mesh->controlIndex = index;
+                        index += nu;
+                        mesh->stateIndex = index;
+                        index += nx;
+                        previousControlMesh = mesh;
+
+                        //Saving dynamical constraints bounds
+                        lowerBoundMap.segment(constraintIndex, nx).setZero();
+                        upperBoundMap.segment(constraintIndex, nx).setZero();
+
+                        //Saving the jacobian structure due to the dynamical constraints
+                        addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
+                        addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
+                        addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
+                        if ((mesh - 1)->origin != first){
+                            addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+                        }
+                        constraintIndex += nx;
 
-            std::vector<MeshPoint>::iterator mesh = m_pimpl->meshPoints.begin(), previousControlMesh = mesh;
-            size_t index = 0, constraintIndex = 0;
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            while (mesh != m_pimpl->meshPointsEnd){
-                if (mesh->origin == first){
-                    //setting up the indeces
-                    mesh->controlIndex = index;
-                    mesh->previousControlIndex = index;
-                    index += nu;
-                    previousControlMesh = mesh;
+                        //Saving constraints bounds
+                        if (!(m_ocproblem->getConstraintsLowerBound(mesh->time, m_minusInfinity, m_constraintsBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
+                            reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
+                        }
+                        lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
 
-                    //Saving constraints bounds
-                    if (!(m_pimpl->ocproblem->getConstraintsLowerBound(mesh->time, m_pimpl->minusInfinity, m_pimpl->constraintsBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
-                        reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
-                    }
-                    lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_pimpl->constraintsBuffer);
+                        if (!(m_ocproblem->getConstraintsUpperBound(mesh->time, m_plusInfinity, m_constraintsBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
+                            reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
+                        }
+                        upperBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
 
-                    if (!(m_pimpl->ocproblem->getConstraintsUpperBound(mesh->time, m_pimpl->plusInfinity, m_pimpl->constraintsBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
-                        reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
-                    }
-                    upperBoundMap.segment(constraintIndex, nc) = toEigen(m_pimpl->constraintsBuffer);
+                        //Saving the jacobian structure due to the constraints
+                        addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
+                        addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        constraintIndex += nc;
 
-                    //Saving the jacobian structure due to the constraints
-                    m_pimpl->addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
-                    constraintIndex += nc;
+                        //Saving the hessian structure
+                        addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                        addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
-                    //Saving the hessian structure
-                    m_pimpl->addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-
-                } else if (mesh->type == MeshPointType::Control) {
-                    mesh->previousControlIndex = previousControlMesh->controlIndex;
-                    mesh->controlIndex = index;
-                    index += nu;
-                    mesh->stateIndex = index;
-                    index += nx;
-                    previousControlMesh = mesh;
-
-                    //Saving dynamical constraints bounds
-                    lowerBoundMap.segment(constraintIndex, nx).setZero();
-                    upperBoundMap.segment(constraintIndex, nx).setZero();
-
-                    //Saving the jacobian structure due to the dynamical constraints
-                    m_pimpl->addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
-                    m_pimpl->addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
-                    m_pimpl->addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
-                    if ((mesh - 1)->origin != first){
-                        m_pimpl->addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
-                    }
-                    constraintIndex += nx;
+                        addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
+                        addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
 
-                    //Saving constraints bounds
-                    if (!(m_pimpl->ocproblem->getConstraintsLowerBound(mesh->time, m_pimpl->minusInfinity, m_pimpl->constraintsBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
-                        reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
-                    }
-                    lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_pimpl->constraintsBuffer);
+                        addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
+                        addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
 
-                    if (!(m_pimpl->ocproblem->getConstraintsUpperBound(mesh->time, m_pimpl->plusInfinity, m_pimpl->constraintsBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
-                        reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
-                    }
-                    upperBoundMap.segment(constraintIndex, nc) = toEigen(m_pimpl->constraintsBuffer);
+                        if ((mesh - 1)->origin != first){
+                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+                        }
 
-                    //Saving the jacobian structure due to the constraints
-                    m_pimpl->addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
-                    m_pimpl->addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
-                    constraintIndex += nc;
 
-                    //Saving the hessian structure
-                    m_pimpl->addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-                    m_pimpl->addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
+                    } else if (mesh->type == MeshPointType::State) {
+                        mesh->controlIndex = previousControlMesh->controlIndex;
+                        mesh->previousControlIndex = previousControlMesh->controlIndex;
+                        mesh->stateIndex = index;
+                        index += nx;
+
+                        //Saving dynamical constraints bounds
+                        lowerBoundMap.segment(constraintIndex, nx).setZero();
+                        upperBoundMap.segment(constraintIndex, nx).setZero();
+
+                        //Saving the jacobian structure due to the dynamical constraints
+                        addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
+                        addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
+                        if ((mesh - 1)->origin != first){
+                            addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+                        }
+                        constraintIndex += nx;
 
-                    m_pimpl->addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                    m_pimpl->addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+                        //Saving constraints bounds
+                        if (!(m_ocproblem->getConstraintsLowerBound(mesh->time, m_minusInfinity, m_constraintsBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
+                            reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
+                        }
+                        lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
 
-                    m_pimpl->addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
-                    m_pimpl->addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
+                        if (!(m_ocproblem->getConstraintsUpperBound(mesh->time, m_plusInfinity, m_constraintsBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
+                            reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
+                        }
+                        upperBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
 
-                    if ((mesh - 1)->origin != first){
-                        m_pimpl->addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
-                        m_pimpl->addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
-                    }
+                        addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
+                        addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        constraintIndex += nc;
 
+                        //Saving the hessian structure
+                        addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                        addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
-                } else if (mesh->type == MeshPointType::State) {
-                    mesh->controlIndex = previousControlMesh->controlIndex;
-                    mesh->previousControlIndex = previousControlMesh->controlIndex;
-                    mesh->stateIndex = index;
-                    index += nx;
+                        addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
+                        addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
 
-                    //Saving dynamical constraints bounds
-                    lowerBoundMap.segment(constraintIndex, nx).setZero();
-                    upperBoundMap.segment(constraintIndex, nx).setZero();
+                        if ((mesh - 1)->origin != first){
+                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+                        }
 
-                    //Saving the jacobian structure due to the dynamical constraints
-                    m_pimpl->addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
-                    m_pimpl->addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
-                    if ((mesh - 1)->origin != first){
-                        m_pimpl->addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
                     }
-                    constraintIndex += nx;
+                    ++mesh;
+                }
+                assert(index == m_numberOfVariables);
+                assert(constraintIndex == m_numberOfConstraints);
 
-                    //Saving constraints bounds
-                    if (!(m_pimpl->ocproblem->getConstraintsLowerBound(mesh->time, m_pimpl->minusInfinity, m_pimpl->constraintsBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
-                        reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
-                    }
-                    lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_pimpl->constraintsBuffer);
+                m_prepared = true;
+                return true;
+            }
 
-                    if (!(m_pimpl->ocproblem->getConstraintsUpperBound(mesh->time, m_pimpl->plusInfinity, m_pimpl->constraintsBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
-                        reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
-                    }
-                    upperBoundMap.segment(constraintIndex, nc) = toEigen(m_pimpl->constraintsBuffer);
+            virtual void reset() override {
+                m_prepared = false;
+                m_totalMeshes = 0;
+                m_controlMeshes = 0;
+                m_numberOfVariables = 0;
+                resetNonZerosCount();
+                resetMeshPoints();
+            }
 
-                    m_pimpl->addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
-                    m_pimpl->addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
-                    constraintIndex += nc;
+            virtual unsigned int numberOfVariables() override {
+                return static_cast<unsigned int>(m_numberOfVariables);
+            }
 
-                    //Saving the hessian structure
-                    m_pimpl->addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-                    m_pimpl->addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
+            virtual unsigned int numberOfConstraints() override {
+                return static_cast<unsigned int>(m_numberOfConstraints);
+            }
 
-                    m_pimpl->addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                    m_pimpl->addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+            virtual bool getConstraintsBounds(VectorDynSize& constraintsLowerBounds, VectorDynSize& constraintsUpperBounds) override {
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "getConstraintsInfo", "First you need to call the prepare method");
+                    return false;
+                }
 
-                    if ((mesh - 1)->origin != first){
-                        m_pimpl->addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
-                        m_pimpl->addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
-                    }
+                if (constraintsLowerBounds.size() != numberOfConstraints()) {
+                    constraintsLowerBounds.resize(numberOfConstraints());
+                }
 
+                if (constraintsUpperBounds.size() != numberOfConstraints()) {
+                    constraintsUpperBounds.resize(numberOfConstraints());
                 }
-                ++mesh;
-            }
-            assert(index == m_pimpl->numberOfVariables);
-            assert(constraintIndex == m_pimpl->numberOfConstraints);
 
-            m_pimpl->prepared = true;
-            return true;
-        }
+                constraintsLowerBounds = m_constraintsLowerBound;
+                constraintsUpperBounds = m_constraintsUpperBound;
 
-        void MultipleShootingTranscription::reset()
-        {
-            m_pimpl->prepared = false;
-            m_pimpl->totalMeshes = 0;
-            m_pimpl->controlMeshes = 0;
-            m_pimpl->numberOfVariables = 0;
-            m_pimpl->resetNonZerosCount();
-            m_pimpl->resetMeshPoints();
-        }
-
-        unsigned int MultipleShootingTranscription::numberOfVariables()
-        {
-            return static_cast<unsigned int>(m_pimpl->numberOfVariables);
-        }
+                return true;
+            }
 
-        unsigned int MultipleShootingTranscription::numberOfConstraints()
-        {
-            return static_cast<unsigned int>(m_pimpl->numberOfConstraints);
-        }
+            virtual bool getVariablesUpperBound(VectorDynSize& variablesUpperBound) override {
 
-        bool MultipleShootingTranscription::getConstraintsBounds(VectorDynSize &constraintsLowerBounds, VectorDynSize &constraintsUpperBounds)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "getConstraintsInfo", "First you need to call the prepare method");
-                return false;
-            }
+                bool stateBounded = true, controlBounded = true;
 
-            if (constraintsLowerBounds.size() != numberOfConstraints()) {
-                constraintsLowerBounds.resize(numberOfConstraints());
-            }
+                Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
+                Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
 
-            if (constraintsUpperBounds.size() != numberOfConstraints()) {
-                constraintsUpperBounds.resize(numberOfConstraints());
-            }
+                if (!(m_ocproblem->getStateUpperBound(m_stateBuffer))) {
+                    stateBounded = false;
+                    stateBufferMap.setConstant(m_plusInfinity);
+                }
 
-            constraintsLowerBounds = m_pimpl->constraintsLowerBound;
-            constraintsUpperBounds = m_pimpl->constraintsUpperBound;
+                if (!(m_ocproblem->getControlUpperBound(m_controlBuffer))) {
+                    controlBounded = false;
+                    controlBufferMap.setConstant(m_plusInfinity);
+                }
 
-            return true;
-        }
+                if (!controlBounded && !stateBounded) {
+                    return false;
+                }
 
-        bool MultipleShootingTranscription::getVariablesUpperBound(VectorDynSize &variablesUpperBound)
-        {
-            bool stateBounded = true, controlBounded = true;
+                if (variablesUpperBound.size() != m_numberOfVariables) {
+                    variablesUpperBound.resize(numberOfVariables());
+                }
+                Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(variablesUpperBound);
 
-            Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_pimpl->stateBuffer);
-            Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_pimpl->controlBuffer);
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
-            if (!(m_pimpl->ocproblem->getStateUpperBound(m_pimpl->stateBuffer))) {
-                stateBounded = false;
-                stateBufferMap.setConstant(m_pimpl->plusInfinity);
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                        upperBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
+                    } else if (mesh->type == MeshPointType::Control) {
+                        upperBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
+                        upperBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                    } else if (mesh->type == MeshPointType::State) {
+                        upperBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                    }
+                }
+                return true;
             }
 
-            if (!(m_pimpl->ocproblem->getControlUpperBound(m_pimpl->controlBuffer))) {
-                controlBounded = false;
-                controlBufferMap.setConstant(m_pimpl->plusInfinity);
-            }
+            virtual bool getVariablesLowerBound(VectorDynSize& variablesLowerBound) override {
 
-            if (!controlBounded && !stateBounded) {
-                return false;
-            }
+                bool stateBounded = true, controlBounded = true;
 
-            if (variablesUpperBound.size() != m_pimpl->numberOfVariables) {
-                variablesUpperBound.resize(m_pimpl->numberOfVariables);
-            }
-            Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(variablesUpperBound);
+                Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
+                Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
+                if (!(m_ocproblem->getStateLowerBound(m_stateBuffer))) {
+                    stateBounded = false;
+                    stateBufferMap.setConstant(m_minusInfinity);
+                }
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                    upperBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
-                } else if (mesh->type == MeshPointType::Control) {
-                    upperBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
-                    upperBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
-                } else if (mesh->type == MeshPointType::State) {
-                    upperBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                if (!(m_ocproblem->getControlLowerBound(m_controlBuffer))) {
+                    controlBounded = false;
+                    controlBufferMap.setConstant(m_minusInfinity);
                 }
-            }
-            return true;
-        }
 
-        bool MultipleShootingTranscription::getVariablesLowerBound(VectorDynSize &variablesLowerBound)
-        {
-            bool stateBounded = true, controlBounded = true;
+                if (!controlBounded && !stateBounded) {
+                    return false;
+                }
 
-            Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_pimpl->stateBuffer);
-            Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_pimpl->controlBuffer);
+                if (variablesLowerBound.size() != numberOfVariables()) {
+                    variablesLowerBound.resize(numberOfVariables());
+                }
+                Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(variablesLowerBound);
 
-            if (!(m_pimpl->ocproblem->getStateLowerBound(m_pimpl->stateBuffer))) {
-                stateBounded = false;
-                stateBufferMap.setConstant(m_pimpl->minusInfinity);
-            }
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
-            if (!(m_pimpl->ocproblem->getControlLowerBound(m_pimpl->controlBuffer))) {
-                controlBounded = false;
-                controlBufferMap.setConstant(m_pimpl->minusInfinity);
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                        lowerBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
+                    } else if (mesh->type == MeshPointType::Control) {
+                        lowerBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
+                        lowerBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                    } else if (mesh->type == MeshPointType::State) {
+                        lowerBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                    }
+                }
+                return true;
             }
 
-            if (!controlBounded && !stateBounded) {
-                return false;
-            }
+            virtual bool getConstraintsJacobianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
 
-            if (variablesLowerBound.size() != m_pimpl->numberOfVariables) {
-                variablesLowerBound.resize(m_pimpl->numberOfVariables);
-            }
-            Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(variablesLowerBound);
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "getConstraintsInfo", "First you need to call the prepare method");
+                    return false;
+                }
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
+                if (nonZeroElementRows.size() != m_jacobianNonZeros) {
+                    nonZeroElementRows.resize(static_cast<unsigned int>(m_jacobianNonZeros));
+                }
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                    lowerBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
-                } else if (mesh->type == MeshPointType::Control) {
-                    lowerBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
-                    lowerBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
-                } else if (mesh->type == MeshPointType::State) {
-                    lowerBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                if (nonZeroElementColumns.size() != m_jacobianNonZeros) {
+                    nonZeroElementColumns.resize(static_cast<unsigned int>(m_jacobianNonZeros));
                 }
-            }
-            return true;
-        }
 
-        bool MultipleShootingTranscription::getConstraintsJacobianInfo(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "getConstraintsInfo", "First you need to call the prepare method");
-                return false;
-            }
 
-            if (nonZeroElementRows.size() != m_pimpl->jacobianNonZeros) {
-                nonZeroElementRows.resize(static_cast<unsigned int>(m_pimpl->jacobianNonZeros));
-            }
+                for (unsigned int i = 0; i < m_jacobianNonZeros; ++i){ //not the full vector of nonZeros should be read
+                    nonZeroElementRows[i] = m_jacobianNZRows[i];
+                    nonZeroElementColumns[i] = m_jacobianNZCols[i];
+                }
 
-            if (nonZeroElementColumns.size() != m_pimpl->jacobianNonZeros) {
-                nonZeroElementColumns.resize(static_cast<unsigned int>(m_pimpl->jacobianNonZeros));
+                return true;
             }
 
+            virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "getHessianInfo", "First you need to call the prepare method");
+                    return false;
+                }
 
-            for (unsigned int i = 0; i < m_pimpl->jacobianNonZeros; ++i){
-                nonZeroElementRows[i] = m_pimpl->jacobianNZRows[i];
-                nonZeroElementColumns[i] = m_pimpl->jacobianNZCols[i];
-            }
+                if (nonZeroElementRows.size() != m_hessianNonZeros) {
+                    nonZeroElementRows.resize(static_cast<unsigned int>(m_hessianNonZeros));
+                }
 
-            return true;
-        }
+                if (nonZeroElementColumns.size() != m_hessianNonZeros) {
+                    nonZeroElementColumns.resize(static_cast<unsigned int>(m_hessianNonZeros));
+                }
 
-        bool MultipleShootingTranscription::getHessianInfo(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "getHessianInfo", "First you need to call the prepare method");
-                return false;
-            }
+                for (unsigned int i = 0; i < m_hessianNonZeros; ++i){
+                    nonZeroElementRows[i] = m_hessianNZRows[i];
+                    nonZeroElementColumns[i] = m_hessianNZCols[i];
+                }
 
-            if (nonZeroElementRows.size() != m_pimpl->hessianNonZeros) {
-                nonZeroElementRows.resize(static_cast<unsigned int>(m_pimpl->hessianNonZeros));
+                return true;
             }
 
-            if (nonZeroElementColumns.size() != m_pimpl->hessianNonZeros) {
-                nonZeroElementColumns.resize(static_cast<unsigned int>(m_pimpl->hessianNonZeros));
-            }
+            virtual bool setVariables(const VectorDynSize& variables) override {
+                if (variables.size() != m_variablesBuffer.size()){
+                    reportError("MultipleShootingTranscription", "setVariables", "The input variables have a size different from the expected one.");
+                    return false;
+                }
 
-            for (unsigned int i = 0; i < m_pimpl->hessianNonZeros; ++i){
-                nonZeroElementRows[i] = m_pimpl->hessianNZRows[i];
-                nonZeroElementColumns[i] = m_pimpl->hessianNZCols[i];
+                m_variablesBuffer = variables;
+                return true;
             }
 
-            return true;
-        }
+            virtual bool evaluateCostFunction(double& costValue) override {
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "evaluateCostFunction", "First you need to call the prepare method");
+                    return false;
+                }
 
-        bool MultipleShootingTranscription::setVariables(const VectorDynSize &variables)
-        {
-            if (variables.size() != m_pimpl->variablesBuffer.size()){
-                reportError("MultipleShootingTranscription", "setVariables", "The input variables have a size different from the expected one.");
-                return false;
-            }
+                Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
+                Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
+                Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_variablesBuffer);
 
-            m_pimpl->variablesBuffer = variables;
-            return true;
-        }
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
-        bool MultipleShootingTranscription::evaluateCostFunction(double &costValue)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "evaluateCostFunction", "First you need to call the prepare method");
-                return false;
-            }
+                costValue = 0;
+                double singleCost;
 
-            Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_pimpl->stateBuffer);
-            Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_pimpl->controlBuffer);
-            Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_pimpl->variablesBuffer);
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                        stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                    } else {
+                        stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
+                    }
+                    controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
+                    if (!(m_ocproblem->costsEvaluation(mesh->time, m_stateBuffer, m_controlBuffer, singleCost))){
+                        std::ostringstream errorMsg;
+                        errorMsg << "Error while evaluating cost at time t = " << mesh->time << ".";
+                        reportError("MultipleShootingTranscription", "evaluateCostFunction", errorMsg.str().c_str());
+                        return false;
+                    }
 
-            costValue = 0;
-            double singleCost;
+                    costValue += singleCost;
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                    stateBufferMap = toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
-                } else {
-                    stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
                 }
-                controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
+                return true;
+            }
 
-                if (!(m_pimpl->ocproblem->costsEvaluation(mesh->time, m_pimpl->stateBuffer, m_pimpl->controlBuffer, singleCost))){
-                    std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating cost at time t = " << mesh->time << ".";
-                    reportError("MultipleShootingTranscription", "evaluateCostFunction", errorMsg.str().c_str());
+            virtual bool evaluateCostGradient(VectorDynSize& gradient) override {
+
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "evaluateCostGradient", "First you need to call the prepare method");
                     return false;
                 }
 
-                costValue += singleCost;
-
-            }
-            return true;
-
-        }
+                Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
+                Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
+                Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_variablesBuffer);
+                Eigen::Map<Eigen::VectorXd> costStateGradient = toEigen(m_costStateGradientBuffer);
+                Eigen::Map<Eigen::VectorXd> costControlGradient = toEigen(m_costControlGradientBuffer);
 
-        bool MultipleShootingTranscription::evaluateCostGradient(VectorDynSize &gradient)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "evaluateCostGradient", "First you need to call the prepare method");
-                return false;
-            }
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
-            Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_pimpl->stateBuffer);
-            Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_pimpl->controlBuffer);
-            Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_pimpl->variablesBuffer);
-            Eigen::Map<Eigen::VectorXd> costStateGradient = toEigen(m_pimpl->costStateGradientBuffer);
-            Eigen::Map<Eigen::VectorXd> costControlGradient = toEigen(m_pimpl->costControlGradientBuffer);
+                if (gradient.size() != numberOfVariables()) {
+                    gradient.resize(static_cast<unsigned int>(numberOfVariables()));
+                }
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
+                Eigen::Map<Eigen::VectorXd> gradientMap = toEigen(gradient);
 
-            if (gradient.size() != m_pimpl->numberOfVariables) {
-                gradient.resize(static_cast<unsigned int>(m_pimpl->numberOfVariables));
-            }
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                        stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                    } else {
+                        stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
+                    }
+                    controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
 
-            Eigen::Map<Eigen::VectorXd> gradientMap = toEigen(gradient);
+                    if (mesh->origin != first){
+                        if (!(m_ocproblem->costsFirstPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costStateGradientBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating cost state gradient at time t = " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateCostGradient", errorMsg.str().c_str());
+                            return false;
+                        }
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                    stateBufferMap = toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
-                } else {
-                    stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
-                }
-                controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
+                        gradientMap.segment(mesh->stateIndex, nx) = costStateGradient;
+                    }
 
-                if (mesh->origin != first){
-                    if (!(m_pimpl->ocproblem->costsFirstPartialDerivativeWRTState(mesh->time, m_pimpl->stateBuffer, m_pimpl->controlBuffer, m_pimpl->costStateGradientBuffer))){
+                    if (!(m_ocproblem->costsFirstPartialDerivativeWRTControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costControlGradientBuffer))){
                         std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating cost state gradient at time t = " << mesh->time << ".";
+                        errorMsg << "Error while evaluating cost control gradient at time t = " << mesh->time << ".";
                         reportError("MultipleShootingTranscription", "evaluateCostGradient", errorMsg.str().c_str());
                         return false;
                     }
 
-                    gradientMap.segment(mesh->stateIndex, nx) = costStateGradient;
-                }
+                    if (mesh->type == MeshPointType::Control){
+                        gradientMap.segment(mesh->controlIndex, nu) = costControlGradient;
+                    } else if (mesh->type == MeshPointType::State) {
+                        gradientMap.segment(mesh->controlIndex, nu) += costControlGradient;
+                    }
 
-                if (!(m_pimpl->ocproblem->costsFirstPartialDerivativeWRTControl(mesh->time, m_pimpl->stateBuffer, m_pimpl->controlBuffer, m_pimpl->costControlGradientBuffer))){
-                    std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating cost control gradient at time t = " << mesh->time << ".";
-                    reportError("MultipleShootingTranscription", "evaluateCostGradient", errorMsg.str().c_str());
-                    return false;
                 }
 
-                if (mesh->type == MeshPointType::Control){
-                    gradientMap.segment(mesh->controlIndex, nu) = costControlGradient;
-                } else if (mesh->type == MeshPointType::State) {
-                    gradientMap.segment(mesh->controlIndex, nu) += costControlGradient;
+                return true;
+            }
+
+            virtual bool evaluateCostHessian(MatrixDynSize& hessian) override {
+
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "evaluateCostHessian", "First you need to call the prepare method");
+                    return false;
                 }
 
-            }
+                Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
+                Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
+                Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_variablesBuffer);
+                iDynTreeEigenMatrixMap costStateHessian = toEigen(m_costHessianStateBuffer);
+                iDynTreeEigenMatrixMap costControlHessian = toEigen(m_costHessianControlBuffer);
+                iDynTreeEigenMatrixMap costStateControlHessian = toEigen(m_costHessianStateControlBuffer);
+                iDynTreeEigenMatrixMap costControlStateHessian = toEigen(m_costHessianControlStateBuffer);
 
-            return true;
-        }
 
-        bool MultipleShootingTranscription::evaluateCostHessian(MatrixDynSize &hessian)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "evaluateCostHessian", "First you need to call the prepare method");
-                return false;
-            }
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
-            Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_pimpl->stateBuffer);
-            Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_pimpl->controlBuffer);
-            Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_pimpl->variablesBuffer);
-            iDynTreeEigenMatrixMap costStateHessian = toEigen(m_pimpl->costHessianStateBuffer);
-            iDynTreeEigenMatrixMap costControlHessian = toEigen(m_pimpl->costHessianControlBuffer);
-            iDynTreeEigenMatrixMap costStateControlHessian = toEigen(m_pimpl->costHessianStateControlBuffer);
-            iDynTreeEigenMatrixMap costControlStateHessian = toEigen(m_pimpl->costHessianControlStateBuffer);
+                if ((hessian.rows() != numberOfVariables()) || (hessian.cols() != numberOfVariables())) {
+                    hessian.resize(static_cast<unsigned int>(numberOfVariables()), static_cast<unsigned int>(numberOfVariables()));
+                }
 
+                iDynTreeEigenMatrixMap hessianMap = toEigen(hessian);
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                        stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                    } else {
+                        stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
+                    }
+                    controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
 
-            if ((hessian.rows() != m_pimpl->numberOfVariables) || (hessian.cols() != m_pimpl->numberOfVariables)) {
-                hessian.resize(static_cast<unsigned int>(m_pimpl->numberOfVariables), static_cast<unsigned int>(m_pimpl->numberOfVariables));
-            }
+                    if (mesh->origin != first){
+                        if (!(m_ocproblem->costsSecondPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating cost state hessian at time t = " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
+                            return false;
+                        }
 
-            iDynTreeEigenMatrixMap hessianMap = toEigen(hessian);
+                        hessianMap.block(mesh->stateIndex, mesh->stateIndex, nx, nx) = costStateHessian;
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                    stateBufferMap = toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
-                } else {
-                    stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
-                }
-                controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
+                        if (!(m_ocproblem->costsSecondPartialDerivativeWRTStateControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateControlBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating cost state-control hessian at time t = " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
+                            return false;
+                        }
 
-                if (mesh->origin != first){
-                    if (!(m_pimpl->ocproblem->costsSecondPartialDerivativeWRTState(mesh->time, m_pimpl->stateBuffer, m_pimpl->controlBuffer, m_pimpl->costHessianStateBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating cost state hessian at time t = " << mesh->time << ".";
-                        reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
-                        return false;
+                        hessianMap.block(mesh->stateIndex, mesh->controlIndex, nx, nu) = costStateControlHessian;
+                        costControlStateHessian = costStateControlHessian.transpose();
+                        hessianMap.block(mesh->controlIndex, mesh->stateIndex, nu, nx) = costControlStateHessian;
                     }
 
-                    hessianMap.block(mesh->stateIndex, mesh->stateIndex, nx, nx) = costStateHessian;
-
-                    if (!(m_pimpl->ocproblem->costsSecondPartialDerivativeWRTStateControl(mesh->time, m_pimpl->stateBuffer, m_pimpl->controlBuffer, m_pimpl->costHessianStateControlBuffer))){
+                    if (!(m_ocproblem->costsSecondPartialDerivativeWRTControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianControlBuffer))){
                         std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating cost state-control hessian at time t = " << mesh->time << ".";
+                        errorMsg << "Error while evaluating cost control hessian at time t = " << mesh->time << ".";
                         reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
                         return false;
                     }
 
-                    hessianMap.block(mesh->stateIndex, mesh->controlIndex, nx, nu) = costStateControlHessian;
-                    costControlStateHessian = costStateControlHessian.transpose();
-                    hessianMap.block(mesh->controlIndex, mesh->stateIndex, nu, nx) = costControlStateHessian;
+                    if (mesh->type == MeshPointType::Control){
+                        hessianMap.block(mesh->controlIndex, mesh->controlIndex, nu, nu) = costControlHessian;
+                    } else if (mesh->type == MeshPointType::State) {
+                        hessianMap.block(mesh->controlIndex, mesh->controlIndex, nu, nu) += costControlHessian;
+                    }
                 }
+                return true;
+            }
 
-                if (!(m_pimpl->ocproblem->costsSecondPartialDerivativeWRTControl(mesh->time, m_pimpl->stateBuffer, m_pimpl->controlBuffer, m_pimpl->costHessianControlBuffer))){
-                    std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating cost control hessian at time t = " << mesh->time << ".";
-                    reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
+            virtual bool evaluateConstraints(VectorDynSize& constraints) override {
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "evaluateConstraints", "First you need to call the prepare method");
                     return false;
                 }
 
-                if (mesh->type == MeshPointType::Control){
-                    hessianMap.block(mesh->controlIndex, mesh->controlIndex, nu, nu) = costControlHessian;
-                } else if (mesh->type == MeshPointType::State) {
-                    hessianMap.block(mesh->controlIndex, mesh->controlIndex, nu, nu) += costControlHessian;
-                }
-            }
-            return true;
-        }
-
-        bool MultipleShootingTranscription::evaluateConstraints(VectorDynSize &constraints)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "evaluateConstraints", "First you need to call the prepare method");
-                return false;
-            }
-
-            Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_pimpl->stateBuffer);
-            Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_pimpl->variablesBuffer);
-            Eigen::Map<Eigen::VectorXd> constraintsBufferMap = toEigen(m_pimpl->constraintsBuffer);
-            Eigen::Map<Eigen::VectorXd> currentState = toEigen(m_pimpl->collocationStateBuffer[1]);
-            Eigen::Map<Eigen::VectorXd> previousState = toEigen(m_pimpl->collocationStateBuffer[0]);
-            Eigen::Map<Eigen::VectorXd> currentControl = toEigen(m_pimpl->collocationControlBuffer[1]);
-            Eigen::Map<Eigen::VectorXd> previousControl = toEigen(m_pimpl->collocationControlBuffer[0]);
+                Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
+                Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_variablesBuffer);
+                Eigen::Map<Eigen::VectorXd> constraintsBufferMap = toEigen(m_constraintsBuffer);
+                Eigen::Map<Eigen::VectorXd> currentState = toEigen(m_collocationStateBuffer[1]);
+                Eigen::Map<Eigen::VectorXd> previousState = toEigen(m_collocationStateBuffer[0]);
+                Eigen::Map<Eigen::VectorXd> currentControl = toEigen(m_collocationControlBuffer[1]);
+                Eigen::Map<Eigen::VectorXd> previousControl = toEigen(m_collocationControlBuffer[0]);
 
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
-            Eigen::Index nc = static_cast<Eigen::Index>(m_pimpl->constraintsPerInstant);
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
+                Eigen::Index nc = static_cast<Eigen::Index>(m_constraintsPerInstant);
 
-            if (constraints.size() != m_pimpl->numberOfConstraints) {
-                constraints.resize(static_cast<unsigned int>(m_pimpl->numberOfConstraints));
-            }
+                if (constraints.size() != numberOfConstraints()) {
+                    constraints.resize(static_cast<unsigned int>(numberOfConstraints()));
+                }
 
-            Eigen::Map<Eigen::VectorXd> constraintsMap = toEigen(constraints);
+                Eigen::Map<Eigen::VectorXd> constraintsMap = toEigen(constraints);
 
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            Eigen::Index constraintIndex = 0;
-            double dT = 0;
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                     currentState= toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
-                } else {
-                    currentState = variablesBuffer.segment(mesh->stateIndex, nx);
-                    if ((mesh -1)->origin == first){
-                        previousState = toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                Eigen::Index constraintIndex = 0;
+                double dT = 0;
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                         currentState= toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        previousState = variablesBuffer.segment((mesh - 1)->stateIndex, nx);
+                        currentState = variablesBuffer.segment(mesh->stateIndex, nx);
+                        if ((mesh -1)->origin == first){
+                            previousState = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                        } else {
+                            previousState = variablesBuffer.segment((mesh - 1)->stateIndex, nx);
+                        }
+                    }
+                    currentControl  = variablesBuffer.segment(mesh->controlIndex, nu);
+                    previousControl = variablesBuffer.segment(mesh->previousControlIndex, nu);
+
+                    if (mesh->origin != first){
+                        dT = mesh->time - (mesh - 1)->time;
+                        if (!(m_integrator->evaluateCollocationConstraint(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT, m_stateBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating the collocation constraint at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateConstraints", errorMsg.str().c_str());
+                            return false;
+                        }
+                        constraintsMap.segment(constraintIndex, nx) = stateBufferMap;
+                        constraintIndex += nx;
                     }
-                }
-                currentControl  = variablesBuffer.segment(mesh->controlIndex, nu);
-                previousControl = variablesBuffer.segment(mesh->previousControlIndex, nu);
 
-                if (mesh->origin != first){
-                    dT = mesh->time - (mesh - 1)->time;
-                    if (!(m_pimpl->integrator->evaluateCollocationConstraint(mesh->time, m_pimpl->collocationStateBuffer, m_pimpl->collocationControlBuffer, dT, m_pimpl->stateBuffer))){
+                    if (!(m_ocproblem->constraintsEvaluation(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsBuffer))){
                         std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating the collocation constraint at time " << mesh->time << ".";
+                        errorMsg << "Error while evaluating the constraints at time " << mesh->time << ".";
                         reportError("MultipleShootingTranscription", "evaluateConstraints", errorMsg.str().c_str());
                         return false;
                     }
-                    constraintsMap.segment(constraintIndex, nx) = stateBufferMap;
-                    constraintIndex += nx;
+                    constraintsMap.segment(constraintIndex, nc) = constraintsBufferMap;
+                    constraintIndex += nc;
                 }
+                assert(static_cast<size_t>(constraintIndex) == numberOfConstraints());
+                return true;
+            }
 
-                if (!(m_pimpl->ocproblem->constraintsEvaluation(mesh->time, m_pimpl->collocationStateBuffer[1], m_pimpl->collocationControlBuffer[1], m_pimpl->constraintsBuffer))){
-                    std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating the constraints at time " << mesh->time << ".";
-                    reportError("MultipleShootingTranscription", "evaluateConstraints", errorMsg.str().c_str());
+            virtual bool evaluateConstraintsJacobian(MatrixDynSize& jacobian) override {
+
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "evaluateConstraints", "First you need to call the prepare method");
                     return false;
                 }
-                constraintsMap.segment(constraintIndex, nc) = constraintsBufferMap;
-                constraintIndex += nc;
-            }
-            assert(static_cast<size_t>(constraintIndex) == m_pimpl->numberOfConstraints);
-            return true;
-        }
-
-        bool MultipleShootingTranscription::evaluateConstraintsJacobian(MatrixDynSize &jacobian)
-        {
-            if (!(m_pimpl->prepared)){
-                reportError("MultipleShootingTranscription", "evaluateConstraints", "First you need to call the prepare method");
-                return false;
-            }
 
-            Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_pimpl->variablesBuffer);
-            Eigen::Map<Eigen::VectorXd> currentState = toEigen(m_pimpl->collocationStateBuffer[1]);
-            Eigen::Map<Eigen::VectorXd> previousState = toEigen(m_pimpl->collocationStateBuffer[0]);
-            Eigen::Map<Eigen::VectorXd> currentControl = toEigen(m_pimpl->collocationControlBuffer[1]);
-            Eigen::Map<Eigen::VectorXd> previousControl = toEigen(m_pimpl->collocationControlBuffer[0]);
+                Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_variablesBuffer);
+                Eigen::Map<Eigen::VectorXd> currentState = toEigen(m_collocationStateBuffer[1]);
+                Eigen::Map<Eigen::VectorXd> previousState = toEigen(m_collocationStateBuffer[0]);
+                Eigen::Map<Eigen::VectorXd> currentControl = toEigen(m_collocationControlBuffer[1]);
+                Eigen::Map<Eigen::VectorXd> previousControl = toEigen(m_collocationControlBuffer[0]);
 
 
-            Eigen::Index nx = static_cast<Eigen::Index>(m_pimpl->nx);
-            Eigen::Index nu = static_cast<Eigen::Index>(m_pimpl->nu);
-            Eigen::Index nc = static_cast<Eigen::Index>(m_pimpl->constraintsPerInstant);
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
+                Eigen::Index nc = static_cast<Eigen::Index>(m_constraintsPerInstant);
 
-            if (jacobian.rows() != m_pimpl->numberOfConstraints || jacobian.cols() != m_pimpl->numberOfVariables) {
-                jacobian.resize(static_cast<unsigned int>(m_pimpl->numberOfConstraints), static_cast<unsigned int>(m_pimpl->numberOfVariables));
-            }
+                if (jacobian.rows() != numberOfConstraints() || jacobian.cols() != numberOfVariables()) {
+                    jacobian.resize(numberOfConstraints(), numberOfVariables());
+                }
 
-            iDynTreeEigenMatrixMap jacobianMap = toEigen(jacobian);
+                iDynTreeEigenMatrixMap jacobianMap = toEigen(jacobian);
 
-            MeshPointOrigin first = MeshPointOrigin::FirstPoint();
-            Eigen::Index constraintIndex = 0;
-            double dT = 0;
-            for (auto mesh = m_pimpl->meshPoints.begin(); mesh != m_pimpl->meshPointsEnd; ++mesh){
-                if (mesh->origin == first){
-                     currentState= toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
-                } else {
-                    currentState = variablesBuffer.segment(mesh->stateIndex, nx);
-                    if ((mesh -1)->origin == first){
-                        previousState = toEigen(m_pimpl->ocproblem->dynamicalSystem().lock()->initialState());
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                Eigen::Index constraintIndex = 0;
+                double dT = 0;
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                         currentState= toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        previousState = variablesBuffer.segment((mesh - 1)->stateIndex, nx);
+                        currentState = variablesBuffer.segment(mesh->stateIndex, nx);
+                        if ((mesh -1)->origin == first){
+                            previousState = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                        } else {
+                            previousState = variablesBuffer.segment((mesh - 1)->stateIndex, nx);
+                        }
                     }
-                }
-                currentControl  = variablesBuffer.segment(mesh->controlIndex, nu);
-                previousControl = variablesBuffer.segment(mesh->previousControlIndex, nu);
+                    currentControl  = variablesBuffer.segment(mesh->controlIndex, nu);
+                    previousControl = variablesBuffer.segment(mesh->previousControlIndex, nu);
+
+                    if (mesh->origin != first){
+                        dT = mesh->time - (mesh - 1)->time;
+                        if (!(m_integrator->evaluateCollocationConstraintJacobian(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT, m_collocationStateJacBuffer, m_collocationControlJacBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating the collocation constraint jacobian at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        if ((mesh -1)->origin != first) {
+                            jacobianMap.block(constraintIndex, (mesh-1)->stateIndex, nx, nx) = toEigen(m_collocationStateJacBuffer[0]);
+                        }
+
+                        jacobianMap.block(constraintIndex, mesh->stateIndex, nx, nx) = toEigen(m_collocationStateJacBuffer[1]);
 
-                if (mesh->origin != first){
-                    dT = mesh->time - (mesh - 1)->time;
-                    if (!(m_pimpl->integrator->evaluateCollocationConstraintJacobian(mesh->time, m_pimpl->collocationStateBuffer, m_pimpl->collocationControlBuffer, dT, m_pimpl->collocationStateJacBuffer, m_pimpl->collocationControlJacBuffer))){
+                        jacobianMap.block(constraintIndex, mesh->controlIndex, nx, nu) = toEigen(m_collocationControlJacBuffer[1]);
+
+                        if (mesh->type == MeshPointType::Control) {
+                            jacobianMap.block(constraintIndex, mesh->previousControlIndex, nx, nu) = toEigen(m_collocationControlJacBuffer[0]);
+                        } else if (mesh->type == MeshPointType::State) {
+                            jacobianMap.block(constraintIndex, mesh->previousControlIndex, nx, nu) += toEigen(m_collocationControlJacBuffer[0]); //the previous and the current control coincides
+                        }
+                        constraintIndex += nx;
+                    }
+
+                    if (!(m_ocproblem->constraintsJacobianWRTState(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsStateJacBuffer))){
                         std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating the collocation constraint jacobian at time " << mesh->time << ".";
+                        errorMsg << "Error while evaluating the constraints state jacobian at time " << mesh->time << ".";
                         reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
                         return false;
                     }
 
-                    if ((mesh -1)->origin != first) {
-                        jacobianMap.block(constraintIndex, (mesh-1)->stateIndex, nx, nx) = toEigen(m_pimpl->collocationStateJacBuffer[0]);
+                    if (mesh->origin != first) {
+                        jacobianMap.block(constraintIndex, mesh->stateIndex, nc, nx) = toEigen(m_constraintsStateJacBuffer);
                     }
 
-                    jacobianMap.block(constraintIndex, mesh->stateIndex, nx, nx) = toEigen(m_pimpl->collocationStateJacBuffer[1]);
-
-                    jacobianMap.block(constraintIndex, mesh->controlIndex, nx, nu) = toEigen(m_pimpl->collocationControlJacBuffer[1]);
-
-                    if (mesh->type == MeshPointType::Control) {
-                        jacobianMap.block(constraintIndex, mesh->previousControlIndex, nx, nu) = toEigen(m_pimpl->collocationControlJacBuffer[0]);
-                    } else if (mesh->type == MeshPointType::State) {
-                        jacobianMap.block(constraintIndex, mesh->previousControlIndex, nx, nu) += toEigen(m_pimpl->collocationControlJacBuffer[0]); //the previous and the current control coincides
+                    if (!(m_ocproblem->constraintsJacobianWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsControlJacBuffer))){
+                        std::ostringstream errorMsg;
+                        errorMsg << "Error while evaluating the constraints control jacobian at time " << mesh->time << ".";
+                        reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
+                        return false;
                     }
-                    constraintIndex += nx;
-                }
-
-                if (!(m_pimpl->ocproblem->constraintsJacobianWRTState(mesh->time, m_pimpl->collocationStateBuffer[1], m_pimpl->collocationControlBuffer[1], m_pimpl->constraintsStateJacBuffer))){
-                    std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating the constraints state jacobian at time " << mesh->time << ".";
-                    reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
-                    return false;
-                }
 
-                if (mesh->origin != first) {
-                    jacobianMap.block(constraintIndex, mesh->stateIndex, nc, nx) = toEigen(m_pimpl->constraintsStateJacBuffer);
-                }
-
-                if (!(m_pimpl->ocproblem->constraintsJacobianWRTControl(mesh->time, m_pimpl->collocationStateBuffer[1], m_pimpl->collocationControlBuffer[1], m_pimpl->constraintsControlJacBuffer))){
-                    std::ostringstream errorMsg;
-                    errorMsg << "Error while evaluating the constraints control jacobian at time " << mesh->time << ".";
-                    reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
-                    return false;
+                    jacobianMap.block(constraintIndex, mesh->controlIndex, nc, nu) = toEigen(m_constraintsControlJacBuffer);
+                    constraintIndex += nc;
                 }
-
-                jacobianMap.block(constraintIndex, mesh->controlIndex, nc, nu) = toEigen(m_pimpl->constraintsControlJacBuffer);
-                constraintIndex += nc;
+                assert(static_cast<size_t>(constraintIndex) == m_numberOfConstraints);
+                return true;
             }
-            assert(static_cast<size_t>(constraintIndex) == m_pimpl->numberOfConstraints);
-            return true;
-        }
 
-        bool MultipleShootingTranscription::evaluateConstraintsHessian(const VectorDynSize &constraintsMultipliers, MatrixDynSize &hessian)
-        {
-            if (!(toEigen(constraintsMultipliers).isZero(0))){
-                reportWarning("MultipleShootingTranscription", "evaluateConstraintsHessian", "The constraints hessian is currently unavailable.");
+            virtual bool evaluateConstraintsHessian(const VectorDynSize& constraintsMultipliers, MatrixDynSize& hessian) override {
+                if (!(toEigen(constraintsMultipliers).isZero(0))){
+                    reportWarning("MultipleShootingTranscription", "evaluateConstraintsHessian", "The constraints hessian is currently unavailable.");
+                }
+                hessian.zero();
+                return true;
             }
-            hessian.zero();
-            return true;
-        }
-
+        };
 
         // MARK: Class implementation
 
@@ -1677,7 +1646,7 @@ namespace iDynTree {
                 return false;
             }
             if (m_optimizer->solve()){
-                if (!(m_optimizer->getPrimalVariables(m_transcription->m_pimpl->solution))){
+                if (!(m_optimizer->getPrimalVariables(m_transcription->m_solution))){
                     reportError("MultipleShootingSolver", "solve", "Error while retrieving the primal variables from the optimizer.");
                     return false;
                 }
@@ -1685,7 +1654,7 @@ namespace iDynTree {
                 reportError("MultipleShootingSolver", "solve", "Error when calling the optimizer solve method.");
                 return false;
             }
-            m_transcription->m_pimpl->solved = true;
+            m_transcription->m_solved = true;
             return true;
         }
 

From e71030da755c0c1acb5c3bb5a1576da174213ab6 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Jun 2018 17:19:11 +0200
Subject: [PATCH 023/194] Removed warnings about types in
 MultipleShootingSolver.

---
 .../OCSolvers/MultipleShootingSolver.h        |   1 -
 .../src/MultipleShootingSolver.cpp            | 107 +++++++++---------
 2 files changed, 54 insertions(+), 54 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
index fc21d818c0d..e44e8741673 100644
--- a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
+++ b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
@@ -18,7 +18,6 @@
 #define IDYNTREE_OPTIMALCONTROL_MULTIPLESHOOTINGSOLVER_H
 
 #include <iDynTree/OptimalControlSolver.h>
-#include <iDynTree/OptimizationProblem.h>
 #include <iDynTree/Optimizer.h>
 
 #include <vector>
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index e168c9811f1..6f7c4fb238e 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -20,6 +20,7 @@
 #include <iDynTree/DynamicalSystem.h>
 #include <iDynTree/Integrator.h>
 #include <iDynTree/TimeRange.h>
+#include <iDynTree/OptimizationProblem.h>
 
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/Utils.h>
@@ -810,18 +811,18 @@ namespace iDynTree {
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin != first){
                         if (states[stateIndex].size() !=  m_nx) {
-                            states[stateIndex].resize(m_nx);
+                            states[stateIndex].resize(static_cast<unsigned int>(m_nx));
                         }
 
-                        toEigen(states[stateIndex]) = solutionMap.segment(mesh->stateIndex, m_nx);
+                        toEigen(states[stateIndex]) = solutionMap.segment(static_cast<unsigned int>(mesh->stateIndex), static_cast<unsigned int>(m_nx));
                         stateIndex++;
                     }
                     if (mesh->type == MeshPointType::Control){
                         if (controls[controlIndex].size() !=  m_nu) {
-                            controls[controlIndex].resize(m_nu);
+                            controls[controlIndex].resize(static_cast<unsigned int>(m_nu));
                         }
 
-                        toEigen(controls[controlIndex]) = solutionMap.segment(mesh->controlIndex, m_nu);
+                        toEigen(controls[controlIndex]) = solutionMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(m_nu));
                         controlIndex++;
                     }
                 }
@@ -873,8 +874,7 @@ namespace iDynTree {
 
             MultipleShootingTranscription(const MultipleShootingTranscription& other) = delete;
 
-            virtual ~MultipleShootingTranscription() override
-            { }
+            virtual ~MultipleShootingTranscription() override;
 
             virtual bool prepare() override {
 
@@ -923,14 +923,14 @@ namespace iDynTree {
                             errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
                             reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
                         }
-                        lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
                         if (!(m_ocproblem->getConstraintsUpperBound(mesh->time, m_plusInfinity, m_constraintsBuffer))){
                             std::ostringstream errorMsg;
                             errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
                             reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
                         }
-                        upperBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
                         //Saving the jacobian structure due to the constraints
                         addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
@@ -948,8 +948,8 @@ namespace iDynTree {
                         previousControlMesh = mesh;
 
                         //Saving dynamical constraints bounds
-                        lowerBoundMap.segment(constraintIndex, nx).setZero();
-                        upperBoundMap.segment(constraintIndex, nx).setZero();
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
+                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
 
                         //Saving the jacobian structure due to the dynamical constraints
                         addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
@@ -966,14 +966,14 @@ namespace iDynTree {
                             errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
                             reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
                         }
-                        lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
                         if (!(m_ocproblem->getConstraintsUpperBound(mesh->time, m_plusInfinity, m_constraintsBuffer))){
                             std::ostringstream errorMsg;
                             errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
                             reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
                         }
-                        upperBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
                         //Saving the jacobian structure due to the constraints
                         addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
@@ -1003,8 +1003,8 @@ namespace iDynTree {
                         index += nx;
 
                         //Saving dynamical constraints bounds
-                        lowerBoundMap.segment(constraintIndex, nx).setZero();
-                        upperBoundMap.segment(constraintIndex, nx).setZero();
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
+                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
 
                         //Saving the jacobian structure due to the dynamical constraints
                         addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
@@ -1020,14 +1020,14 @@ namespace iDynTree {
                             errorMsg << "Error while evaluating constraints lower bounds at time " << mesh->time << ".";
                             reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
                         }
-                        lowerBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
                         if (!(m_ocproblem->getConstraintsUpperBound(mesh->time, m_plusInfinity, m_constraintsBuffer))){
                             std::ostringstream errorMsg;
                             errorMsg << "Error while evaluating constraints upper bounds at time " << mesh->time << ".";
                             reportError("MultipleShootingSolver", "prepare", errorMsg.str().c_str());
                         }
-                        upperBoundMap.segment(constraintIndex, nc) = toEigen(m_constraintsBuffer);
+                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
                         addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
@@ -1124,12 +1124,12 @@ namespace iDynTree {
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
-                        upperBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
+                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
                     } else if (mesh->type == MeshPointType::Control) {
-                        upperBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
-                        upperBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
                     } else if (mesh->type == MeshPointType::State) {
-                        upperBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
                     }
                 }
                 return true;
@@ -1167,12 +1167,12 @@ namespace iDynTree {
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
-                        lowerBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
                     } else if (mesh->type == MeshPointType::Control) {
-                        lowerBoundMap.segment(mesh->controlIndex, nu) = controlBufferMap;
-                        lowerBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
                     } else if (mesh->type == MeshPointType::State) {
-                        lowerBoundMap.segment(mesh->stateIndex, nx) = stateBufferMap;
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
                     }
                 }
                 return true;
@@ -1255,9 +1255,9 @@ namespace iDynTree {
                     if (mesh->origin == first){
                         stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
+                        stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                     }
-                    controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
+                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
                     if (!(m_ocproblem->costsEvaluation(mesh->time, m_stateBuffer, m_controlBuffer, singleCost))){
                         std::ostringstream errorMsg;
@@ -1299,9 +1299,9 @@ namespace iDynTree {
                     if (mesh->origin == first){
                         stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
+                        stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                     }
-                    controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
+                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
                     if (mesh->origin != first){
                         if (!(m_ocproblem->costsFirstPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costStateGradientBuffer))){
@@ -1311,7 +1311,7 @@ namespace iDynTree {
                             return false;
                         }
 
-                        gradientMap.segment(mesh->stateIndex, nx) = costStateGradient;
+                        gradientMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = costStateGradient;
                     }
 
                     if (!(m_ocproblem->costsFirstPartialDerivativeWRTControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costControlGradientBuffer))){
@@ -1322,9 +1322,9 @@ namespace iDynTree {
                     }
 
                     if (mesh->type == MeshPointType::Control){
-                        gradientMap.segment(mesh->controlIndex, nu) = costControlGradient;
+                        gradientMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = costControlGradient;
                     } else if (mesh->type == MeshPointType::State) {
-                        gradientMap.segment(mesh->controlIndex, nu) += costControlGradient;
+                        gradientMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) += costControlGradient;
                     }
 
                 }
@@ -1362,9 +1362,9 @@ namespace iDynTree {
                     if (mesh->origin == first){
                         stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        stateBufferMap = variablesBuffer.segment(mesh->stateIndex, nx);
+                        stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                     }
-                    controlBufferMap = variablesBuffer.segment(mesh->controlIndex, nu);
+                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
                     if (mesh->origin != first){
                         if (!(m_ocproblem->costsSecondPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateBuffer))){
@@ -1374,7 +1374,7 @@ namespace iDynTree {
                             return false;
                         }
 
-                        hessianMap.block(mesh->stateIndex, mesh->stateIndex, nx, nx) = costStateHessian;
+                        hessianMap.block(static_cast<Eigen::Index>(mesh->stateIndex), static_cast<Eigen::Index>(mesh->stateIndex), nx, nx) = costStateHessian;
 
                         if (!(m_ocproblem->costsSecondPartialDerivativeWRTStateControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateControlBuffer))){
                             std::ostringstream errorMsg;
@@ -1383,9 +1383,9 @@ namespace iDynTree {
                             return false;
                         }
 
-                        hessianMap.block(mesh->stateIndex, mesh->controlIndex, nx, nu) = costStateControlHessian;
+                        hessianMap.block(static_cast<Eigen::Index>(mesh->stateIndex), static_cast<Eigen::Index>(mesh->controlIndex), nx, nu) = costStateControlHessian;
                         costControlStateHessian = costStateControlHessian.transpose();
-                        hessianMap.block(mesh->controlIndex, mesh->stateIndex, nu, nx) = costControlStateHessian;
+                        hessianMap.block(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(mesh->stateIndex), nu, nx) = costControlStateHessian;
                     }
 
                     if (!(m_ocproblem->costsSecondPartialDerivativeWRTControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianControlBuffer))){
@@ -1396,9 +1396,9 @@ namespace iDynTree {
                     }
 
                     if (mesh->type == MeshPointType::Control){
-                        hessianMap.block(mesh->controlIndex, mesh->controlIndex, nu, nu) = costControlHessian;
+                        hessianMap.block(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(mesh->controlIndex), nu, nu) = costControlHessian;
                     } else if (mesh->type == MeshPointType::State) {
-                        hessianMap.block(mesh->controlIndex, mesh->controlIndex, nu, nu) += costControlHessian;
+                        hessianMap.block(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(mesh->controlIndex), nu, nu) += costControlHessian;
                     }
                 }
                 return true;
@@ -1437,15 +1437,15 @@ namespace iDynTree {
                     if (mesh->origin == first){
                          currentState= toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        currentState = variablesBuffer.segment(mesh->stateIndex, nx);
+                        currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                         if ((mesh -1)->origin == first){
                             previousState = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                         } else {
-                            previousState = variablesBuffer.segment((mesh - 1)->stateIndex, nx);
+                            previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
                         }
                     }
-                    currentControl  = variablesBuffer.segment(mesh->controlIndex, nu);
-                    previousControl = variablesBuffer.segment(mesh->previousControlIndex, nu);
+                    currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
+                    previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
 
                     if (mesh->origin != first){
                         dT = mesh->time - (mesh - 1)->time;
@@ -1503,15 +1503,15 @@ namespace iDynTree {
                     if (mesh->origin == first){
                          currentState= toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                     } else {
-                        currentState = variablesBuffer.segment(mesh->stateIndex, nx);
+                        currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                         if ((mesh -1)->origin == first){
                             previousState = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                         } else {
-                            previousState = variablesBuffer.segment((mesh - 1)->stateIndex, nx);
+                            previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
                         }
                     }
-                    currentControl  = variablesBuffer.segment(mesh->controlIndex, nu);
-                    previousControl = variablesBuffer.segment(mesh->previousControlIndex, nu);
+                    currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
+                    previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
 
                     if (mesh->origin != first){
                         dT = mesh->time - (mesh - 1)->time;
@@ -1523,17 +1523,17 @@ namespace iDynTree {
                         }
 
                         if ((mesh -1)->origin != first) {
-                            jacobianMap.block(constraintIndex, (mesh-1)->stateIndex, nx, nx) = toEigen(m_collocationStateJacBuffer[0]);
+                            jacobianMap.block(constraintIndex, static_cast<Eigen::Index>((mesh-1)->stateIndex), nx, nx) = toEigen(m_collocationStateJacBuffer[0]);
                         }
 
-                        jacobianMap.block(constraintIndex, mesh->stateIndex, nx, nx) = toEigen(m_collocationStateJacBuffer[1]);
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nx, nx) = toEigen(m_collocationStateJacBuffer[1]);
 
-                        jacobianMap.block(constraintIndex, mesh->controlIndex, nx, nu) = toEigen(m_collocationControlJacBuffer[1]);
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->controlIndex), nx, nu) = toEigen(m_collocationControlJacBuffer[1]);
 
                         if (mesh->type == MeshPointType::Control) {
-                            jacobianMap.block(constraintIndex, mesh->previousControlIndex, nx, nu) = toEigen(m_collocationControlJacBuffer[0]);
+                            jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->previousControlIndex), nx, nu) = toEigen(m_collocationControlJacBuffer[0]);
                         } else if (mesh->type == MeshPointType::State) {
-                            jacobianMap.block(constraintIndex, mesh->previousControlIndex, nx, nu) += toEigen(m_collocationControlJacBuffer[0]); //the previous and the current control coincides
+                            jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->previousControlIndex), nx, nu) += toEigen(m_collocationControlJacBuffer[0]); //the previous and the current control coincides
                         }
                         constraintIndex += nx;
                     }
@@ -1546,7 +1546,7 @@ namespace iDynTree {
                     }
 
                     if (mesh->origin != first) {
-                        jacobianMap.block(constraintIndex, mesh->stateIndex, nc, nx) = toEigen(m_constraintsStateJacBuffer);
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nc, nx) = toEigen(m_constraintsStateJacBuffer);
                     }
 
                     if (!(m_ocproblem->constraintsJacobianWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsControlJacBuffer))){
@@ -1556,7 +1556,7 @@ namespace iDynTree {
                         return false;
                     }
 
-                    jacobianMap.block(constraintIndex, mesh->controlIndex, nc, nu) = toEigen(m_constraintsControlJacBuffer);
+                    jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->controlIndex), nc, nu) = toEigen(m_constraintsControlJacBuffer);
                     constraintIndex += nc;
                 }
                 assert(static_cast<size_t>(constraintIndex) == m_numberOfConstraints);
@@ -1571,6 +1571,7 @@ namespace iDynTree {
                 return true;
             }
         };
+        MultipleShootingSolver::MultipleShootingTranscription::~MultipleShootingTranscription() {}
 
         // MARK: Class implementation
 

From fba499fcf8ba4a5a6c58d0fac9c5970bfd673518 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Jun 2018 19:57:26 +0200
Subject: [PATCH 024/194] Multiple shooting fills the OptimizationProblem data
 structure. Ipopt interface changed to exploit this.

---
 .../include/iDynTree/OptimizationProblem.h    | 12 ++--
 src/optimalcontrol/src/IpoptInterface.cpp     | 66 ++++++++++++++-----
 .../src/MultipleShootingSolver.cpp            | 61 +++++++++++------
 .../src/OptimizationProblem.cpp               | 18 ++---
 4 files changed, 108 insertions(+), 49 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
index 8bea66bf77d..b5f41302982 100644
--- a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
@@ -43,11 +43,11 @@ namespace iDynTree {
 
             bool hasNonLinearConstraints;
 
-            bool hasLinearCost;
+            bool costIsLinear;
 
-            bool hasQuadraticCost;
+            bool costIsQuadratic;
 
-            bool hasNonLinearCost;
+            bool costIsNonLinear;
 
             bool hasSparseConstraintJacobian;
 
@@ -70,11 +70,11 @@ namespace iDynTree {
 
             bool hasNonLinearConstraints() const;
 
-            bool hasLinearCost() const;
+            bool costIsLinear() const;
 
-            bool hasQuadraticCost() const;
+            bool costIsQuadratic() const;
 
-            bool hasNonLinearCost() const;
+            bool costIsNonLinear() const;
 
             bool hasSparseConstraintJacobian() const;
 
diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index ccb0fdcf488..7f9807fbd77 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -520,25 +520,61 @@ namespace iDynTree {
 
             m_pimpl->nlpPointer->numberOfConstraints = m_problem->numberOfConstraints();
 
-            if (!(m_problem->getConstraintsJacobianInfo(m_pimpl->nlpPointer->constraintsJacNNZRows,
-                                                        m_pimpl->nlpPointer->constraintsJacNNZCols))){
-                reportError("IpoptInterface", "solve", "Error while retrieving constraints jacobian info.");
-                return false;
+            if (!(m_problem->info().hessianIsProvided())) {
+                m_pimpl->loader->Options()->SetStringValue("hessian_approximation", "limited-memory");
             }
-            if (!(m_problem->getHessianInfo(m_pimpl->nlpPointer->hessianNNZRows,
-                                            m_pimpl->nlpPointer->hessianNNZCols))){
-                reportError("IpoptInterface", "solve", "Error while retrieving hessian info.");
-                return false;
+
+            if (!(m_problem->info().hasNonLinearConstraints())) {
+                m_pimpl->loader->Options()->SetStringValue("jac_c_constant", "yes");
+                m_pimpl->loader->Options()->SetStringValue("jac_d_constant", "yes");
+
+                if (m_problem->info().costIsQuadratic()) {
+                    m_pimpl->loader->Options()->SetStringValue("hessian_constant", "yes");
+                }
+            }
+
+            if (m_problem->info().hasSparseConstraintJacobian()) {
+                if (!(m_problem->getConstraintsJacobianInfo(m_pimpl->nlpPointer->constraintsJacNNZRows,
+                                                            m_pimpl->nlpPointer->constraintsJacNNZCols))){
+                    reportError("IpoptInterface", "solve", "Error while retrieving constraints jacobian info.");
+                    return false;
+                }
+            } else { //dense jacobian
+                m_pimpl->nlpPointer->constraintsJacNNZCols.clear();
+                m_pimpl->nlpPointer->constraintsJacNNZRows.clear();
+                for (unsigned int i = 0; i < m_pimpl->nlpPointer->numberOfConstraints; i++) {
+                    for (unsigned int j = 0; j < m_pimpl->nlpPointer->numberOfVariables; ++j) {
+                        m_pimpl->nlpPointer->constraintsJacNNZRows.push_back(i);
+                        m_pimpl->nlpPointer->constraintsJacNNZCols.push_back(j);
+                    }
+                }
+            }
+
+            if (m_problem->info().hasSparseHessian()) {
+                if (!(m_problem->getHessianInfo(m_pimpl->nlpPointer->hessianNNZRows,
+                                                m_pimpl->nlpPointer->hessianNNZCols))){
+                    reportError("IpoptInterface", "solve", "Error while retrieving hessian info.");
+                    return false;
+                }
+            } else { //dense hessian
+                m_pimpl->nlpPointer->hessianNNZRows.clear();
+                m_pimpl->nlpPointer->hessianNNZCols.clear();
+                for (unsigned int i = 0; i < m_pimpl->nlpPointer->numberOfVariables; i++) {
+                    for (unsigned int j = 0; j < m_pimpl->nlpPointer->numberOfVariables; ++j) {
+                        m_pimpl->nlpPointer->hessianNNZRows.push_back(i);
+                        m_pimpl->nlpPointer->hessianNNZCols.push_back(j);
+                    }
+                }
             }
 
             if (m_pimpl->possibleReOptimize()){ //reoptimize possibility
-                m_pimpl->loader->Options()->SetStringValue("warm_start_init_point", "yes");
-                m_pimpl->loader->Options()->SetStringValue("warm_start_same_structure", "yes");
-                m_pimpl->loader->Options()->SetNumericValue("warm_start_bound_frac", 1e-6);
-                m_pimpl->loader->Options()->SetNumericValue("warm_start_bound_push", 1e-6);
-                m_pimpl->loader->Options()->SetNumericValue("warm_start_mult_bound_push", 1e-6);
-                m_pimpl->loader->Options()->SetNumericValue("warm_start_slack_bound_frac", 1e-6);
-                m_pimpl->loader->Options()->SetNumericValue("warm_start_slack_bound_push", 1e-6);
+                m_pimpl->loader->Options()->SetStringValueIfUnset("warm_start_init_point", "yes");
+                m_pimpl->loader->Options()->SetStringValueIfUnset("warm_start_same_structure", "yes");
+                m_pimpl->loader->Options()->SetNumericValueIfUnset("warm_start_bound_frac", 1e-6);
+                m_pimpl->loader->Options()->SetNumericValueIfUnset("warm_start_bound_push", 1e-6);
+                m_pimpl->loader->Options()->SetNumericValueIfUnset("warm_start_mult_bound_push", 1e-6);
+                m_pimpl->loader->Options()->SetNumericValueIfUnset("warm_start_slack_bound_frac", 1e-6);
+                m_pimpl->loader->Options()->SetNumericValueIfUnset("warm_start_slack_bound_push", 1e-6);
 
                 m_pimpl->loader->ReOptimizeTNLP(m_pimpl->nlpPointer);
                 if (m_pimpl->nlpPointer->exitCode < 0) {
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 6f7c4fb238e..593c456be8c 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -899,6 +899,16 @@ namespace iDynTree {
                 size_t nc = m_constraintsPerInstant;
                 m_numberOfConstraints = (m_totalMeshes - 1) * nx + (m_constraintsPerInstant) * (m_totalMeshes); //dynamical constraints (removing the initial state) and normal constraints
 
+                //Determine problem type
+                m_infoData->hasLinearConstraints = (m_ocproblem->countLinearConstraints() != 0) || m_ocproblem->systemIsLinear();
+                m_infoData->hasNonLinearConstraints = (m_ocproblem->countLinearConstraints() != m_constraintsPerInstant) || !(m_ocproblem->systemIsLinear());
+                m_infoData->costIsLinear = m_ocproblem->hasOnlyLinearCosts();
+                m_infoData->costIsQuadratic = m_ocproblem->hasOnlyQuadraticCosts();
+                m_infoData->costIsNonLinear = !(m_ocproblem->hasOnlyLinearCosts()) && !(m_ocproblem->hasOnlyQuadraticCosts());
+                m_infoData->hasSparseHessian = true;
+                m_infoData->hasSparseConstraintJacobian = true;
+                m_infoData->hessianIsProvided = !(m_infoData->hasNonLinearConstraints);
+
                 allocateBuffers();
 
                 Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(m_constraintsLowerBound);
@@ -937,7 +947,9 @@ namespace iDynTree {
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                        if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
+                            addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                        }
 
                     } else if (mesh->type == MeshPointType::Control) {
                         mesh->previousControlIndex = previousControlMesh->controlIndex;
@@ -981,18 +993,22 @@ namespace iDynTree {
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-                        addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
+                        if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
+                            addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                            addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
-                        addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                        addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+                            addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
+                            addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
 
-                        addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
-                        addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
+                            addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
+                            addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
+                        }
 
-                        if ((mesh - 1)->origin != first){
-                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
-                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+                        if (!(m_ocproblem->systemIsLinear())) {
+                            if ((mesh - 1)->origin != first){
+                                addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                                addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+                            }
                         }
 
 
@@ -1034,15 +1050,19 @@ namespace iDynTree {
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-                        addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
+                        if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
+                            addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                            addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
-                        addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                        addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+                            addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
+                            addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+                        }
 
-                        if ((mesh - 1)->origin != first){
-                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
-                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+                        if (!(m_ocproblem->systemIsLinear())) {
+                            if ((mesh - 1)->origin != first){
+                                addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                                addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+                            }
                         }
 
                     }
@@ -1564,8 +1584,11 @@ namespace iDynTree {
             }
 
             virtual bool evaluateConstraintsHessian(const VectorDynSize& constraintsMultipliers, MatrixDynSize& hessian) override {
-                if (!(toEigen(constraintsMultipliers).isZero(0))){
-                    reportWarning("MultipleShootingTranscription", "evaluateConstraintsHessian", "The constraints hessian is currently unavailable.");
+                if (m_info.hasNonLinearConstraints()) {
+                    if (!(toEigen(constraintsMultipliers).isZero(0))){
+                        reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", "The constraints hessian is currently unavailable.");
+                        return false;
+                    }
                 }
                 hessian.zero();
                 return true;
diff --git a/src/optimalcontrol/src/OptimizationProblem.cpp b/src/optimalcontrol/src/OptimizationProblem.cpp
index b66698c2ccc..c956db02feb 100644
--- a/src/optimalcontrol/src/OptimizationProblem.cpp
+++ b/src/optimalcontrol/src/OptimizationProblem.cpp
@@ -122,9 +122,9 @@ namespace iDynTree {
         OptimizationProblemInfoData::OptimizationProblemInfoData()
         : hasLinearConstraints(false)
         , hasNonLinearConstraints(true)
-        , hasLinearCost(false)
-        , hasQuadraticCost(false)
-        , hasNonLinearCost(true)
+        , costIsLinear(false)
+        , costIsQuadratic(false)
+        , costIsNonLinear(true)
         , hasSparseConstraintJacobian(false)
         , hasSparseHessian(false)
         , hessianIsProvided(false)
@@ -144,19 +144,19 @@ namespace iDynTree {
             return m_data->hasNonLinearConstraints;
         }
 
-        bool OptimizationProblemInfo::hasLinearCost() const
+        bool OptimizationProblemInfo::costIsLinear() const
         {
-            return m_data->hasLinearCost;
+            return m_data->costIsLinear;
         }
 
-        bool OptimizationProblemInfo::hasQuadraticCost() const
+        bool OptimizationProblemInfo::costIsQuadratic() const
         {
-            return m_data->hasQuadraticCost;
+            return m_data->costIsQuadratic;
         }
 
-        bool OptimizationProblemInfo::hasNonLinearCost() const
+        bool OptimizationProblemInfo::costIsNonLinear() const
         {
-            return m_data->hasNonLinearCost;
+            return m_data->costIsNonLinear;
         }
 
         bool OptimizationProblemInfo::hasSparseConstraintJacobian() const

From 057b7c61f6b5b2385c15ef9c1dd1097b4fa30329 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 8 Jun 2018 09:19:26 +0200
Subject: [PATCH 025/194] Fixed valgrind issue. Unitialized field due to a
 wrong copy/paste.

---
 src/optimalcontrol/src/OptimalControlProblem.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 359c4f7ffd0..7a4cd0b9f15 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -87,7 +87,7 @@ namespace iDynTree {
                 newCost.weight = weight;
                 newCost.timeRange = timeRange;
                 newCost.isLinear = isLinear;
-                newCost.isLinear = isQuadratic;
+                newCost.isQuadratic = isQuadratic;
 
                 std::pair<CostsMap::iterator, bool> costResult;
                 costResult = costs.insert(std::pair<std::string, TimedCost>(cost->name(), newCost));

From cce4992c2874406e67761709bd1ab2285ad56d70 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 8 Jun 2018 11:47:17 +0200
Subject: [PATCH 026/194] The dynamical system for multiple shooting can be
 obtained only from OCProblem.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 13 +++----------
 1 file changed, 3 insertions(+), 10 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 593c456be8c..bfef86e448e 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -366,16 +366,9 @@ namespace iDynTree {
 
                 if (!(m_prepared)){
                     if (m_ocproblem->dynamicalSystem().expired()){
-                        if (m_integrator->dynamicalSystem().expired()){
-                            reportError("MultipleShootingTranscription", "prepare",
-                                        "No dynamical system set, neither to the OptimalControlProblem nor to the Integrator object.");
-                            return false;
-                        }
-                        if (!(m_ocproblem->setDynamicalSystemConstraint(m_integrator->dynamicalSystem().lock()))){
-                            reportError("MultipleShootingTranscription", "prepare",
-                                        "Error while setting the dynamicalSystem to the OptimalControlProblem using the one pointer provided by the Integrator object.");
-                            return false;
-                        }
+                        reportError("MultipleShootingTranscription", "prepare",
+                                    "No dynamical system set to the OptimalControlProblem.");
+                        return false;
                     } else {
                         if (!(m_integrator->dynamicalSystem().expired()) &&
                                 (m_integrator->dynamicalSystem().lock() != m_ocproblem->dynamicalSystem().lock())){

From 021dfb1cd488d0ef0979bcc16fd69903c7e5c8db Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 8 Jun 2018 11:47:52 +0200
Subject: [PATCH 027/194] Added possibility to update the selectors of L2Norm
 costs.

---
 .../include/iDynTree/L2NormCost.h             |  4 +++
 src/optimalcontrol/src/L2NormCost.cpp         | 31 +++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index b53806bfd55..65851e5249f 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -63,6 +63,10 @@ namespace iDynTree {
 
             bool setControlDesiredTrajectory(std::shared_ptr<TimeVaryingVector> controlDesiredTrajectory);
 
+            bool updatStateSelector(const MatrixDynSize& stateSelector);
+
+            bool updatControlSelector(const MatrixDynSize& controlSelector);
+
         };
 
     }
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index ee31734c42b..c6d6f5fa8d3 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -101,6 +101,17 @@ namespace iDynTree {
                 return m_outputVector;
             }
 
+            bool updateSelector(const MatrixDynSize& selector) {
+                if ((selector.rows() != m_selectorMatrix.rows()) || (selector.cols() != m_selectorMatrix.cols())) {
+                    reportError("TimeVaryingGradient", "getObject", "The new selector dimensionsions do not match the old one.");
+                    return false;
+                }
+                m_selectorMatrix = selector;
+                toEigen(m_subMatrix) = toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+
+                return true;
+            }
+
             const MatrixDynSize& selector() {
                 return m_selectorMatrix;
             }
@@ -374,6 +385,26 @@ namespace iDynTree {
             return m_pimpl->controlGradient->setDesiredTrajectory(controlDesiredTrajectory);
         }
 
+        bool L2NormCost::updatStateSelector(const MatrixDynSize &stateSelector)
+        {
+            if (!(m_pimpl->stateGradient->updateSelector(stateSelector))) {
+                reportError("L2NormCost", "updatStateSelector", "Error when updating state selector.");
+                return false;
+            }
+            toEigen(m_pimpl->stateHessian->get()) = toEigen(m_pimpl->stateGradient->selector()).transpose() * toEigen(m_pimpl->stateGradient->subMatrix());
+            return true;
+        }
+
+        bool L2NormCost::updatControlSelector(const MatrixDynSize &controlSelector)
+        {
+            if (!(m_pimpl->controlGradient->updateSelector(controlSelector))) {
+                reportError("L2NormCost", "updatControlSelector", "Error when updating state selector.");
+                return false;
+            }
+            toEigen(m_pimpl->controlHessian->get()) = toEigen(m_pimpl->controlGradient->selector()).transpose() * toEigen(m_pimpl->controlGradient->subMatrix());
+            return true;
+        }
+
     }
 }
 

From 0e94a4a3fd59a380893c2c20f74bbffb61853bbb Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 11 Jun 2018 15:37:57 +0200
Subject: [PATCH 028/194] Added OSQP interface.

---
 src/optimalcontrol/CMakeLists.txt             |  16 +
 .../iDynTree/Optimizers/OsqpInterface.h       | 100 +++
 src/optimalcontrol/src/OsqpInterface.cpp      | 844 ++++++++++++++++++
 3 files changed, 960 insertions(+)
 create mode 100644 src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
 create mode 100644 src/optimalcontrol/src/OsqpInterface.cpp

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index f21667e7dd0..417d0f245a8 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -72,6 +72,22 @@ if (IDYNTREE_USES_IPOPT)
     list(APPEND INCLUDE_LIST ${IPOPT_INCLUDE_DIRS})
 endif()
 
+find_package(OsqpEigen QUIET)
+if (${OsqpEigen_FOUND})
+    option(OC_USE_OSQP "Use OSQP as a solver in optimalcontrol." ON)
+else()
+    set(OC_USE_OSQP OFF)
+endif()
+
+if (OC_USE_OSQP)
+    list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/OsqpInterface.h)
+    list(APPEND OPTIMIZERS_SOURCES src/OsqpInterface.cpp)
+    list(APPEND LINK_LIST   OsqpEigen::OsqpEigen osqp::osqp)
+endif()
+
+
+
+
 add_library(${libraryname} ${PUBLIC_HEADERS} ${INTEGRATORS_PUBLIC_HEADERS} ${OCSOLVERS_PUBLIC_HEADERS} ${OPTIMIZERS_HEADERS} ${SOURCES} ${OPTIMIZERS_SOURCES})
 target_include_directories(${libraryname} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
 target_include_directories(${libraryname} SYSTEM PRIVATE ${INCLUDE_LIST})
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
new file mode 100644
index 00000000000..c309534ffe8
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#ifndef IDYNTREE_OPTIMALCONTROL_OSQPINTERFACE_H
+#define IDYNTREE_OPTIMALCONTROL_OSQPINTERFACE_H
+
+#include <memory>
+#include <string>
+#include <iDynTree/Optimizer.h>
+
+namespace iDynTree {
+
+    class VectorDynSize;
+
+    namespace optimization {
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+
+        typedef struct {
+            double rho = 0.1; //>0
+            double sigma = 1e-06; //>0, when changed prevent reOptimize
+            unsigned int max_iter = 4000; // >0
+            double eps_abs = 1e-03; //>=0
+            double eps_rel = 1e-03; //>=0
+            double eps_prim_inf = 1e-04; //>=0
+            double eps_dual_inf  = 1e-04; //>=0
+            double alpha = 1.6; // 0 < x < 2
+            unsigned int linsys_solver = 0; //0/1, when changed prevent reOptimize
+            double delta = 1e-06; //<0
+            bool polish = false;
+            unsigned int polish_refine_iter = 3; //>0
+            bool verbose = true;
+            bool scaled_termination = false;
+            unsigned int check_termination = 25;
+            bool warm_start = true;
+            unsigned int scaling = 10; //when changed prevent reOptimize
+            bool adaptive_rho = true; // when changed prevent reOptimize
+            unsigned int adaptive_rho_interval = 0; // when changed prevent reOptimize
+            double adaptive_rho_tolerance = 5; //>=1, when changed prevent reOptimize
+            double adaptive_rho_fraction = 0.4; //>0// when changed prevent reOptimize
+            double time_limit = 0; //>=0
+        } OsqpSettings;
+
+        class OsqpInterface : public Optimizer {
+
+            class OsqpInterfaceImplementation;
+            OsqpInterfaceImplementation *m_pimpl;
+
+        public:
+
+            OsqpInterface();
+
+            OsqpInterface(const OsqpInterface &other) = delete;
+
+            virtual ~OsqpInterface() override;
+
+            virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
+
+            virtual bool setInitialGuess(VectorDynSize &initialGuess) override;
+
+            virtual bool solve() override;
+
+            virtual bool getPrimalVariables(VectorDynSize &primalVariables) override;
+
+            virtual bool getDualVariables(VectorDynSize &constraintsMultipliers,
+                                          VectorDynSize &lowerBoundsMultipliers,
+                                          VectorDynSize &upperBoundsMultipliers) override;
+
+            virtual bool getOptimalCost(double &optimalCost) override;
+
+            virtual bool getOptimalConstraintsValues(VectorDynSize &constraintsValues) override;
+
+            virtual double minusInfinity() override;
+
+            virtual double plusInfinity() override;
+
+            const OsqpSettings& settings() const;
+        };
+
+    }
+
+}
+
+#endif // IDYNTREE_OPTIMALCONTROL_OSQPINTERFACE_H
diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
new file mode 100644
index 00000000000..575957387dc
--- /dev/null
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -0,0 +1,844 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/OsqpInterface.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Sparse>
+#include <iDynTree/Core/EigenHelpers.h>
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/Utils.h>
+
+#include <cassert>
+#include <ostream>
+
+#include <OsqpEigen/OsqpEigen.h>
+
+#include <vector>
+#include <iterator>
+
+namespace iDynTree {
+    namespace optimization {
+
+        class Triplet {
+        public:
+            size_t m_row, m_col;
+            double m_value;
+            const size_t &row() const {
+                return m_row;
+            }
+
+            const size_t &col() const {
+                return m_col;
+            }
+
+            const double& value() const {
+                return m_value;
+            }
+        };
+
+        class TripletIterator : public std::iterator<std::input_iterator_tag, Triplet> {
+            Triplet m_triplet;
+            std::shared_ptr<std::vector<size_t>> m_rowIndeces;
+            std::shared_ptr<std::vector<size_t>> m_colIndeces;
+            std::shared_ptr<MatrixDynSize> m_denseMatrix;
+            size_t m_nnzIdentity, m_nnzIndex;
+            bool m_addIdentity;
+
+        public:
+            TripletIterator(std::shared_ptr<std::vector<size_t>> rowIndeces,
+                            std::shared_ptr<std::vector<size_t>> colIndeces,
+                            std::shared_ptr<MatrixDynSize> denseMatrix,
+                            bool addIdentityOnTop = false)
+            : m_rowIndeces(rowIndeces)
+            , m_colIndeces(colIndeces)
+            , m_denseMatrix(denseMatrix)
+            , m_nnzIdentity(0)
+            , m_nnzIndex(0)
+            , m_addIdentity(addIdentityOnTop)
+            {
+            }
+
+            TripletIterator& operator++() {
+                if (m_addIdentity && (m_nnzIdentity < m_denseMatrix->cols())) {
+                    m_nnzIdentity++;
+                } else {
+                    m_nnzIndex++;
+                }
+                return *this;
+            }
+
+            bool  operator==(const TripletIterator& rhs) const{
+                return ((rhs.m_nnzIndex == this->m_nnzIndex) && (rhs.m_nnzIdentity == this->m_nnzIdentity));
+            }
+
+            bool operator!=(const TripletIterator& rhs) const {
+                return ((rhs.m_nnzIndex != this->m_nnzIndex) || (rhs.m_nnzIdentity != this->m_nnzIdentity));
+            }
+
+            Triplet* operator->() {
+                return &(operator*());
+            }
+
+            Triplet& operator*() {
+                if (m_addIdentity && (m_nnzIdentity < m_denseMatrix->cols())) {
+                    m_triplet.m_row = m_nnzIdentity;
+                    m_triplet.m_col = m_nnzIdentity;
+                    m_triplet.m_value = 1.0;
+                    return m_triplet;
+                }
+
+                assert(m_nnzIndex < m_colIndeces->size());
+                m_triplet.m_row = m_rowIndeces->operator[](m_nnzIndex);
+                m_triplet.m_col = m_colIndeces->operator[](m_nnzIndex);
+                unsigned int row = static_cast<unsigned int>(m_triplet.m_row);
+                unsigned int col = static_cast<unsigned int>(m_triplet.m_col);
+                m_triplet.m_value = m_denseMatrix->operator()(row, col);
+                return m_triplet;
+            }
+
+            static TripletIterator begin(std::shared_ptr<std::vector<size_t>> rowIndeces,
+                                         std::shared_ptr<std::vector<size_t>> colIndeces,
+                                         std::shared_ptr<MatrixDynSize> denseMatrix,
+                                         bool addIdentityOnTop = false) {
+                TripletIterator m_begin(rowIndeces, colIndeces, denseMatrix, addIdentityOnTop);
+                return m_begin;
+            }
+
+            static TripletIterator end(std::shared_ptr<std::vector<size_t>> rowIndeces,
+                                       std::shared_ptr<std::vector<size_t>> colIndeces,
+                                       std::shared_ptr<MatrixDynSize> denseMatrix,
+                                       bool addIdentityOnTop = false) {
+                TripletIterator m_end(rowIndeces, colIndeces, denseMatrix, addIdentityOnTop);
+                m_end.m_nnzIdentity = denseMatrix->cols();
+                m_end.m_nnzIndex = rowIndeces->size();
+                return m_end;
+            }
+
+        };
+
+        class DenseIterator : public std::iterator<std::input_iterator_tag, Triplet> {
+            Triplet m_triplet;
+            std::shared_ptr<MatrixDynSize> m_denseMatrix;
+            size_t m_nnzIdentity, m_rowIndex, m_colIndex;
+            bool m_addIdentity;
+
+        public:
+            DenseIterator(std::shared_ptr<MatrixDynSize> denseMatrix,
+                          bool addIdentityOnTop = false)
+            : m_denseMatrix(denseMatrix)
+            , m_nnzIdentity(0)
+            , m_rowIndex(0)
+            , m_colIndex(0)
+            , m_addIdentity(addIdentityOnTop)
+            {
+            }
+
+            DenseIterator& operator++() {
+                if (m_addIdentity && (m_nnzIdentity < m_denseMatrix->cols())) {
+                    m_nnzIdentity++;
+                } else {
+                    if (m_rowIndex < m_denseMatrix->rows()) {
+                        m_rowIndex++;
+                    } else {
+                        if (m_colIndex < m_denseMatrix->cols()) {
+                            m_colIndex++;
+                            m_rowIndex = 0;
+                        }
+                    }
+                }
+                return *this;
+            }
+
+            bool  operator==(const DenseIterator& rhs) const{
+                return ((rhs.m_rowIndex == this->m_rowIndex) && (rhs.m_colIndex == this->m_colIndex) && (rhs.m_nnzIdentity == this->m_nnzIdentity));
+            }
+
+            bool operator!=(const DenseIterator& rhs) const {
+                return !(operator==(rhs));
+            }
+
+            Triplet* operator->() {
+                return &(operator*());
+            }
+
+            Triplet& operator*() {
+                if (m_addIdentity && (m_nnzIdentity < m_denseMatrix->cols())) {
+                    m_triplet.m_row = m_nnzIdentity;
+                    m_triplet.m_col = m_nnzIdentity;
+                    m_triplet.m_value = 1.0;
+                    return m_triplet;
+                }
+
+                assert(m_rowIndex < m_denseMatrix->rows());
+                assert(m_colIndex < m_denseMatrix->cols());
+
+                m_triplet.m_row = m_rowIndex;
+                m_triplet.m_col = m_colIndex;
+                unsigned int row = static_cast<unsigned int>(m_triplet.m_row);
+                unsigned int col = static_cast<unsigned int>(m_triplet.m_col);
+                m_triplet.m_value = m_denseMatrix->operator()(row, col);
+                return m_triplet;
+            }
+
+            static DenseIterator begin(std::shared_ptr<MatrixDynSize> denseMatrix,
+                                       bool addIdentityOnTop = false) {
+                DenseIterator m_begin(denseMatrix, addIdentityOnTop);
+                return m_begin;
+            }
+
+            static DenseIterator end(std::shared_ptr<MatrixDynSize> denseMatrix,
+                                     bool addIdentityOnTop = false) {
+                DenseIterator m_end(denseMatrix, addIdentityOnTop);
+                m_end.m_nnzIdentity = denseMatrix->cols();
+                m_end.m_rowIndex = denseMatrix->rows();
+                m_end.m_colIndex = denseMatrix->cols();
+                return m_end;
+            }
+
+        };
+
+
+        class OsqpInterface::OsqpInterfaceImplementation {
+        public:
+            OsqpSettings settings, previousSettings;
+            OsqpEigen::Solver solver;
+            bool initialGuessSet = false;
+            VectorDynSize constraintsUpperBounds, constraintsLowerBounds;
+            VectorDynSize variablesLowerBounds, variablesUpperBounds; //these bounds needs to be added manually to the constraints
+            bool hasBoxConstraints;
+            std::shared_ptr<std::vector<size_t>> constraintNNZRows, constraintNNZCols;
+            std::shared_ptr<std::vector<size_t>> hessianNNZRows, hessianNNZCols;
+            VectorDynSize variablesBuffer, solutionBuffer;
+            VectorDynSize costGradient;
+            std::shared_ptr<MatrixDynSize> costHessian;
+            std::shared_ptr<MatrixDynSize> constraintJacobian;
+            unsigned int nv, nc, previous_nv, previous_nc;
+
+            Eigen::SparseMatrix<double> eigenHessian;
+            Eigen::SparseMatrix<double> eigenJacobian;
+            Eigen::VectorXd eigenGradient, eigenInitialGuess, unifiedLowerBounds, unifiedUpperBounds;
+            Eigen::VectorXd eigenPrimalVariables, eigenDualVariables;
+            bool alreadySolved = false;
+            bool isLowerBounded = false;
+            bool isUpperBounded = false;
+            bool optionsAllowReoptimize = true;
+
+            bool checkAndSetSetting() {
+                if (!checkDoublesAreEqual(settings.rho, previousSettings.rho)) {
+                    if (settings.rho <= 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "rho has to be >0");
+                        return false;
+                    }
+                    solver.settings()->setRho(settings.rho);
+                }
+
+                if (!checkDoublesAreEqual(settings.sigma, previousSettings.sigma)) {
+                    if (settings.sigma <= 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "sigma has to be >0");
+                        return false;
+                    }
+                    solver.settings()->setSigma(settings.sigma);
+                    optionsAllowReoptimize = false;
+                }
+
+                if (settings.max_iter != previousSettings.max_iter) {
+                    if (settings.max_iter == 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "max_iter has to be >0");
+                        return false;
+                    }
+                    solver.settings()->setMaxIteraction(static_cast<int>(settings.max_iter));
+                }
+
+                if (!checkDoublesAreEqual(settings.eps_abs, previousSettings.eps_abs)) {
+                    if (settings.eps_abs < 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "eps_abs has to be >=0");
+                        return false;
+                    }
+                    solver.settings()->setAbsoluteTolerance(settings.eps_abs);
+                }
+
+                if (!checkDoublesAreEqual(settings.eps_rel, previousSettings.eps_rel)) {
+                    if (settings.eps_rel < 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "eps_rel has to be >=0");
+                        return false;
+                    }
+                    solver.settings()->setRelativeTolerance(settings.eps_rel);
+                }
+
+                if (!checkDoublesAreEqual(settings.eps_prim_inf, previousSettings.eps_prim_inf)) {
+                    if (settings.eps_prim_inf < 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "eps_prim_inf has to be >=0");
+                        return false;
+                    }
+                    solver.settings()->setPrimalInfeasibilityTollerance(settings.eps_prim_inf);
+                }
+
+                if (!checkDoublesAreEqual(settings.eps_dual_inf, previousSettings.eps_dual_inf)) {
+                    if (settings.eps_dual_inf < 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "eps_dual_inf has to be >=0");
+                        return false;
+                    }
+                    solver.settings()->setDualInfeasibilityTollerance(settings.eps_dual_inf);
+                }
+
+                if (!checkDoublesAreEqual(settings.alpha, previousSettings.alpha)) {
+                    if ((settings.alpha <= 0) || (settings.alpha >=2)) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "alpha has to be in the range (0,2).");
+                        return false;
+                    }
+                    solver.settings()->setAlpha(settings.alpha);
+                }
+
+                if (settings.linsys_solver != previousSettings.linsys_solver) {
+                    solver.settings()->setLinearSystemSolver(static_cast<int>(settings.linsys_solver));
+                    optionsAllowReoptimize = false;
+                }
+
+                if (!checkDoublesAreEqual(settings.delta, previousSettings.delta)) {
+                    if (settings.delta <= 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "delta has to be >0");
+                        return false;
+                    }
+                    solver.settings()->setDelta(settings.delta);
+                }
+
+                if (settings.polish != previousSettings.polish) {
+                    solver.settings()->setPolish(settings.polish);
+                }
+
+                if (settings.polish_refine_iter != previousSettings.polish_refine_iter) {
+                    if (settings.polish_refine_iter == 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "polish_refine_iter has to be >0");
+                        return false;
+                    }
+                    solver.settings()->setPolishRefineIter(static_cast<int>(settings.polish_refine_iter));
+                }
+
+                if (settings.verbose != previousSettings.verbose) {
+                    solver.settings()->setVerbosity(settings.verbose);
+                }
+
+                if (settings.scaled_termination != previousSettings.scaled_termination) {
+                    solver.settings()->setScaledTerimination(settings.scaled_termination);
+                }
+
+                if (settings.check_termination != previousSettings.check_termination) {
+                    solver.settings()->setCheckTermination(static_cast<int>(settings.check_termination));
+                }
+
+                if (settings.warm_start != previousSettings.warm_start) {
+                    solver.settings()->setWarmStart(settings.warm_start);
+                }
+
+                if (settings.scaling != previousSettings.scaling) {
+                    solver.settings()->setScaling(static_cast<int>(settings.scaling));
+                    optionsAllowReoptimize = false;
+                }
+
+                if (settings.adaptive_rho != previousSettings.adaptive_rho) {
+                    solver.settings()->setAdaptiveRho(settings.adaptive_rho);
+                    optionsAllowReoptimize = false;
+                }
+
+                if (settings.adaptive_rho_interval != previousSettings.adaptive_rho_interval) {
+                    solver.settings()->setAdaptiveRhoInterval(static_cast<int>(settings.adaptive_rho_interval));
+                    optionsAllowReoptimize = false;
+                }
+
+                if (!checkDoublesAreEqual(settings.adaptive_rho_tolerance, previousSettings.adaptive_rho_tolerance)) {
+                    if (settings.adaptive_rho_tolerance < 1) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "adaptive_rho_tolerance has to be >= 1");
+                        return false;
+                    }
+                    solver.settings()->setAdaptiveRhoTolerance(settings.adaptive_rho_tolerance);
+                    optionsAllowReoptimize = false;
+                }
+
+                if (!checkDoublesAreEqual(settings.adaptive_rho_fraction, previousSettings.adaptive_rho_fraction)) {
+                    if (settings.adaptive_rho_fraction <= 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "adaptive_rho_fraction has to be > 0");
+                        return false;
+                    }
+                    solver.settings()->setAdaptiveRhoFraction(settings.adaptive_rho_fraction);
+                    optionsAllowReoptimize = false;
+                }
+
+                if (!checkDoublesAreEqual(settings.time_limit, previousSettings.time_limit)) {
+                    if (settings.time_limit < 0) {
+                        reportError("OsqpSettings", "checkAndSetSetting", "time_limit has to be >= 0");
+                        return false;
+                    }
+                    solver.settings()->getSettings()->time_limit = static_cast<c_float>(settings.time_limit);
+                }
+
+                return true;
+            }
+
+            bool possibleReStart() {
+                bool initialized = solver.isInitialized();
+                bool sameVariables = (previous_nv == nv);
+                bool sameConstraints = (previous_nc == nc);
+                return alreadySolved && optionsAllowReoptimize && initialized && sameVariables && sameConstraints;
+            }
+        };
+
+        OsqpInterface::OsqpInterface()
+        : m_pimpl(new OsqpInterfaceImplementation)
+        {
+            assert(m_pimpl);
+            m_problem = nullptr;
+            m_pimpl->costHessian = std::make_shared<MatrixDynSize>();
+            m_pimpl->constraintJacobian = std::make_shared<MatrixDynSize>();
+            m_pimpl->constraintNNZRows = std::make_shared<std::vector<size_t>>();
+            m_pimpl->constraintNNZCols = std::make_shared<std::vector<size_t>>();
+            m_pimpl->hessianNNZRows = std::make_shared<std::vector<size_t>>();
+            m_pimpl->hessianNNZCols = std::make_shared<std::vector<size_t>>();
+        }
+
+        OsqpInterface::~OsqpInterface()
+        {
+            if (m_pimpl) {
+                delete m_pimpl;
+                m_pimpl = nullptr;
+            }
+        }
+
+        bool OsqpInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+        {
+            if (!problem){
+                reportError("OsqpInterface", "setProblem", "Empty problem pointer.");
+                return false;
+            }
+            m_problem = problem;
+            return true;
+        }
+
+        bool OsqpInterface::setInitialGuess(VectorDynSize &initialGuess)
+        {
+            m_pimpl->initialGuessSet = true;
+            m_pimpl->eigenInitialGuess.resize(initialGuess.size());
+            m_pimpl->eigenInitialGuess = toEigen(initialGuess);
+            return true;
+        }
+
+        bool OsqpInterface::solve()
+        {
+            if (!m_problem) {
+                reportError("OsqpInterface", "solve", "Optimization problem not set.");
+                return false;
+            }
+
+            if (!(m_problem->info().costIsQuadratic()) || (m_problem->info().hasNonLinearConstraints())) {
+                reportError("OsqpInterface", "solve", "The specified optimization problem cannot be solved by this solver. It needs to be a QP.");
+                return false;
+            }
+
+            if (!(m_problem->info().hessianIsProvided())) {
+                reportError("OsqpInterface", "solve", "Hessian must be provided. Only the cost hessian will be evaluated.");
+                return false;
+            }
+
+            if (!m_problem->prepare()){
+                reportError("OsqpInterface", "solve", "Error while preparing the optimization problem.");
+                return false;
+            }
+
+            m_pimpl->nv = m_problem->numberOfVariables();
+
+            m_pimpl->isLowerBounded = m_problem->getVariablesLowerBound(m_pimpl->variablesLowerBounds);
+            m_pimpl->isUpperBounded = m_problem->getVariablesUpperBound(m_pimpl->variablesUpperBounds);
+
+            if (m_pimpl->isLowerBounded || m_pimpl->isUpperBounded) {
+                m_pimpl->hasBoxConstraints = true;
+                if (!(m_pimpl->isLowerBounded)) {
+                    m_pimpl->variablesLowerBounds.resize(m_pimpl->nv);
+                    toEigen(m_pimpl->variablesLowerBounds).setConstant(minusInfinity());
+                }
+                if (!(m_pimpl->isUpperBounded)) {
+                    m_pimpl->variablesUpperBounds.resize(m_pimpl->nv);
+                    toEigen(m_pimpl->variablesUpperBounds).setConstant(plusInfinity());
+                }
+            } else {
+                m_pimpl->hasBoxConstraints = false;
+            }
+
+            m_pimpl->nc = m_problem->numberOfConstraints();
+
+            if (m_pimpl->hasBoxConstraints) {
+                m_pimpl->nc += m_pimpl->nv;
+            }
+
+            if (!(m_problem->getConstraintsBounds(m_pimpl->constraintsLowerBounds, m_pimpl->constraintsUpperBounds))) {
+                reportError("OsqpInterface", "solve", "Error while retrieving constraint bounds.");
+                return false;
+            }
+
+            m_pimpl->unifiedLowerBounds.resize(m_pimpl->nc);
+            m_pimpl->unifiedUpperBounds.resize(m_pimpl->nc);
+            if (m_pimpl->hasBoxConstraints) {
+                m_pimpl->unifiedLowerBounds.head(m_pimpl->nv) = toEigen(m_pimpl->variablesLowerBounds);
+                m_pimpl->unifiedLowerBounds.tail(m_problem->numberOfConstraints()) = toEigen(m_pimpl->constraintsLowerBounds);
+
+                m_pimpl->unifiedUpperBounds.head(m_pimpl->nv) = toEigen(m_pimpl->variablesUpperBounds);
+                m_pimpl->unifiedUpperBounds.tail(m_problem->numberOfConstraints()) = toEigen(m_pimpl->constraintsUpperBounds);
+            } else {
+                m_pimpl->unifiedLowerBounds = toEigen(m_pimpl->constraintsLowerBounds);
+                m_pimpl->unifiedUpperBounds = toEigen(m_pimpl->constraintsUpperBounds);
+            }
+
+            if (m_pimpl->variablesBuffer.size() != m_pimpl->nv) {
+                m_pimpl->variablesBuffer.resize(m_pimpl->nv);
+                m_pimpl->variablesBuffer.zero();
+            }
+
+            if (!(m_problem->setVariables(m_pimpl->variablesBuffer))) {
+                reportError("OsqpInterface", "solve", "Error while setting to optimization variables to the problem.");
+                return false;
+            }
+
+            if (!(m_problem->evaluateCostGradient(m_pimpl->costGradient))) {
+                reportError("OsqpInterface", "solve", "Error while retrieving cost gradient.");
+                return false;
+            }
+
+            m_pimpl->eigenGradient.resize(m_pimpl->nv);
+            m_pimpl->eigenGradient = toEigen(m_pimpl->costGradient);
+
+            if (m_problem->info().hasSparseHessian()) {
+                if (!(m_problem->getHessianInfo(*(m_pimpl->hessianNNZRows), *(m_pimpl->hessianNNZCols)))) {
+                    reportError("OsqpInterface", "solve", "Error while retrieving hessian sparsity structure.");
+                    return false;
+                }
+
+                if (!(m_problem->evaluateCostHessian(*(m_pimpl->costHessian)))) {
+                    reportError("OsqpInterface", "solve", "Error while retrieving cost hessian.");
+                    return false;
+                }
+
+                TripletIterator beginIterator = TripletIterator::begin(m_pimpl->hessianNNZRows,
+                                                                       m_pimpl->hessianNNZCols,
+                                                                       m_pimpl->costHessian);
+
+                TripletIterator endIterator = TripletIterator::end(m_pimpl->hessianNNZRows,
+                                                                   m_pimpl->hessianNNZCols,
+                                                                   m_pimpl->costHessian);
+
+                m_pimpl->eigenHessian.resize(m_pimpl->nv, m_pimpl->nv);
+                m_pimpl->eigenHessian.setFromTriplets(beginIterator, endIterator);
+            } else {
+                if (!(m_problem->evaluateCostHessian(*(m_pimpl->costHessian)))) {
+                    reportError("OsqpInterface", "solve", "Error while retrieving cost hessian.");
+                    return false;
+                }
+
+                DenseIterator beginIterator = DenseIterator::begin(m_pimpl->costHessian);
+
+                DenseIterator endIterator = DenseIterator::end(m_pimpl->costHessian);
+
+                m_pimpl->eigenHessian.resize(m_pimpl->nv, m_pimpl->nv);
+                m_pimpl->eigenHessian.setFromTriplets(beginIterator, endIterator);
+            }
+
+            if (m_problem->info().hasSparseConstraintJacobian()) {
+                if (!(m_problem->getConstraintsJacobianInfo(*(m_pimpl->constraintNNZRows), *(m_pimpl->constraintNNZCols)))) {
+                    reportError("OsqpInterface", "solve", "Error while retrieving constraint jacobian sparsity structure.");
+                    return false;
+                }
+
+                if (!(m_problem->evaluateConstraintsJacobian(*(m_pimpl->constraintJacobian)))) {
+                    reportError("OsqpInterface", "solve", "Error while retrieving constraint jacobian.");
+                    return false;
+                }
+
+                TripletIterator beginIterator = TripletIterator::begin(m_pimpl->constraintNNZRows,
+                                                                       m_pimpl->constraintNNZCols,
+                                                                       m_pimpl->constraintJacobian,
+                                                                       m_pimpl->hasBoxConstraints);
+
+                TripletIterator endIterator = TripletIterator::end(m_pimpl->constraintNNZRows,
+                                                                   m_pimpl->constraintNNZCols,
+                                                                   m_pimpl->constraintJacobian,
+                                                                   m_pimpl->hasBoxConstraints);
+
+                m_pimpl->eigenJacobian.resize(m_pimpl->nc, m_pimpl->nv);
+                m_pimpl->eigenJacobian.setFromTriplets(beginIterator, endIterator);
+            } else {
+                if (!(m_problem->evaluateConstraintsJacobian(*(m_pimpl->constraintJacobian)))) {
+                    reportError("OsqpInterface", "solve", "Error while retrieving constraint jacobian.");
+                    return false;
+                }
+
+                DenseIterator beginIterator = DenseIterator::begin(m_pimpl->constraintJacobian,
+                                                                   m_pimpl->hasBoxConstraints);
+
+                DenseIterator endIterator = DenseIterator::end(m_pimpl->constraintJacobian,
+                                                               m_pimpl->hasBoxConstraints);
+
+                m_pimpl->eigenJacobian.resize(m_pimpl->nc, m_pimpl->nv);
+                m_pimpl->eigenJacobian.setFromTriplets(beginIterator, endIterator);
+            }
+
+            if (!(m_pimpl->checkAndSetSetting())) {
+                reportError("OsqpInterface", "solve", "The specified settings seems to be faulty.");
+                return false;
+            }
+
+            if (m_pimpl->possibleReStart()) {
+
+                if (!m_pimpl->solver.updateHessianMatrix(m_pimpl->eigenHessian)) {
+                    reportError("OsqpInterface", "solve", "Error while updating the cost hessian.");
+                    return false;
+                }
+
+                if (!m_pimpl->solver.updateGradient(m_pimpl->eigenGradient)) {
+                    reportError("OsqpInterface", "solve", "Error while updating the cost gradient.");
+                    return false;
+                }
+
+                if (!m_pimpl->solver.updateLinearConstraintsMatrix(m_pimpl->eigenJacobian)) {
+                    reportError("OsqpInterface", "solve", "Error while updating the constraints jacobian.");
+                    return false;
+                }
+
+                if (!m_pimpl->solver.updateBounds(m_pimpl->unifiedLowerBounds, m_pimpl->unifiedUpperBounds)) {
+                    reportError("OsqpInterface", "solve", "Error while updating the constraints bounds.");
+                    return false;
+                }
+
+                if (m_pimpl->initialGuessSet) {
+                    if (m_pimpl->eigenInitialGuess.size() != m_pimpl->nv) {
+                        reportError("OsqpInterface", "solve", "The specified intial guess has dimension different from the number of variables.");
+                        return false;
+                    }
+
+                    if (!(m_pimpl->solver.setPrimalVariable(m_pimpl->eigenInitialGuess))) {
+                        reportError("OsqpInterface", "solve", "Error while setting the initial guess to the solver.");
+                        return false;
+                    }
+
+                } else {
+                    if (!(m_pimpl->solver.setPrimalVariable(m_pimpl->eigenPrimalVariables))) {
+                        reportError("OsqpInterface", "solve", "Error while setting the initial guess from the previous run to the solver.");
+                        return false;
+                    }
+                }
+
+                if (!(m_pimpl->solver.setDualVariable(m_pimpl->eigenDualVariables))) {
+                    reportError("OsqpInterface", "solve", "Error while setting the initial guess for dual variables to the solver.");
+                    return false;
+                }
+
+            } else {
+                if (m_pimpl->solver.isInitialized()) {
+                    m_pimpl->solver.clearSolver();
+                }
+                m_pimpl->alreadySolved = false;
+
+                m_pimpl->solver.data()->setNumberOfVariables(static_cast<int>(m_pimpl->nv));
+
+                m_pimpl->solver.data()->setNumberOfConstraints(static_cast<int>(m_pimpl->nc));
+
+                if (!(m_pimpl->solver.data()->setHessianMatrix(m_pimpl->eigenHessian)))
+                {
+                    reportError("OsqpInterface", "solve", "Error while setting the hessian matrix during solver initialization.");
+                    return false;
+                }
+
+                if (!(m_pimpl->solver.data()->setGradient(m_pimpl->eigenGradient))) {
+                    reportError("OsqpInterface", "solve", "Error while setting the cost gradient during solver initialization.");
+                    return false;
+                }
+
+                if (!(m_pimpl->solver.data()->setLinearConstraintsMatrix(m_pimpl->eigenJacobian))) {
+                    reportError("OsqpInterface", "solve", "Error while setting the constraints jacobian during solver initialization.");
+                    return false;
+                }
+
+                if (!(m_pimpl->solver.data()->setLowerBound(m_pimpl->unifiedLowerBounds))) {
+                    reportError("OsqpInterface", "solve", "Error while setting the constraints lower bounds during solver initialization.");
+                    return false;
+                }
+
+                if (!(m_pimpl->solver.data()->setUpperBound(m_pimpl->unifiedUpperBounds))) {
+                    reportError("OsqpInterface", "solve", "Error while setting the constraints upper bounds during solver initialization.");
+                    return false;
+                }
+
+                if (!(m_pimpl->solver.initSolver())) {
+                    reportError("OsqpInterface", "solve", "Failed to initialize solver.");
+                    return false;
+                }
+
+                if (m_pimpl->initialGuessSet) {
+                    if (m_pimpl->eigenInitialGuess.size() != m_pimpl->nv) {
+                        reportError("OsqpInterface", "solve", "The specified intial guess has dimension different from the number of variables.");
+                        return false;
+                    }
+
+                    if (!(m_pimpl->solver.setPrimalVariable(m_pimpl->eigenInitialGuess))) {
+                        reportError("OsqpInterface", "solve", "Error while setting the initial guess to the solver.");
+                        return false;
+                    }
+                }
+            }
+
+            if (!(m_pimpl->solver.solve())) {
+                reportError("OsqpInterface", "solve", "Error while solving the QP.");
+                return false;
+            }
+
+            m_pimpl->eigenPrimalVariables = m_pimpl->solver.getSolution();
+            m_pimpl->solutionBuffer.resize(m_pimpl->nv);
+            toEigen(m_pimpl->solutionBuffer) = m_pimpl->eigenPrimalVariables;
+            m_pimpl->solver.getDualVariable(m_pimpl->eigenDualVariables);
+
+            m_pimpl->alreadySolved = true;
+            m_pimpl->initialGuessSet = false;
+            m_pimpl->previous_nv = m_pimpl->nv;
+            m_pimpl->previous_nc = m_pimpl->nc;
+            m_pimpl->previousSettings = m_pimpl->settings;
+            m_pimpl->optionsAllowReoptimize = true;
+            return true;
+        }
+
+        bool OsqpInterface::getPrimalVariables(VectorDynSize &primalVariables)
+        {
+            if (!(m_pimpl->alreadySolved)) {
+                reportError("OsqpInterface", "getPrimalVariables", "First you have to call the method solve once.");
+                return false;
+            }
+            primalVariables = m_pimpl->solutionBuffer;
+            return true;
+        }
+
+        bool OsqpInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
+        {
+            if (!(m_pimpl->alreadySolved)) {
+                reportError("OsqpInterface", "getDualVariables", "First you have to call the method solve once.");
+                return false;
+            }
+
+            constraintsMultipliers.resize(m_pimpl->nc);
+            lowerBoundsMultipliers.resize(m_pimpl->nv);
+            upperBoundsMultipliers.resize(m_pimpl->nv);
+
+            if (!(m_pimpl->hasBoxConstraints)) {
+                lowerBoundsMultipliers.zero();
+                upperBoundsMultipliers.zero();
+                constraintsMultipliers = constraintsMultipliers;
+                return true;
+            }
+
+            toEigen(constraintsMultipliers) = m_pimpl->eigenDualVariables.tail(m_pimpl->nc);
+
+            if (!(m_pimpl->isLowerBounded)) {
+                lowerBoundsMultipliers.zero();
+                toEigen(upperBoundsMultipliers) = m_pimpl->eigenDualVariables.head(m_pimpl->nv);
+                return true;
+            }
+
+            if (!(m_pimpl->isUpperBounded)) {
+                upperBoundsMultipliers.zero();
+                toEigen(lowerBoundsMultipliers) = m_pimpl->eigenDualVariables.head(m_pimpl->nv);
+                return true;
+            }
+
+            unsigned int iDynTreeIndex;
+            double distanceToUpper, distanceToLower;
+            for (Eigen::Index i = 0; i < m_pimpl->nv; ++i) {
+                iDynTreeIndex = static_cast<unsigned int>(i);
+                bool equalityConstraint = checkDoublesAreEqual(m_pimpl->variablesLowerBounds(iDynTreeIndex), m_pimpl->variablesUpperBounds(iDynTreeIndex), 1E-6);
+                bool boundsNotActive = checkDoublesAreEqual(m_pimpl->eigenDualVariables(i), 0.0, 1E-6);
+                if (equalityConstraint || boundsNotActive) {
+                    lowerBoundsMultipliers(iDynTreeIndex) = m_pimpl->eigenDualVariables(i);
+                    upperBoundsMultipliers(iDynTreeIndex) = m_pimpl->eigenDualVariables(i);
+                } else {
+                    distanceToLower = std::abs(m_pimpl->eigenPrimalVariables(i) - m_pimpl->variablesLowerBounds(iDynTreeIndex));
+                    distanceToUpper = std::abs(m_pimpl->eigenPrimalVariables(i) - m_pimpl->variablesUpperBounds(iDynTreeIndex));
+                    if (distanceToLower < distanceToUpper) {
+                        lowerBoundsMultipliers(iDynTreeIndex) = m_pimpl->eigenDualVariables(i);
+                        upperBoundsMultipliers(iDynTreeIndex) = 0.0;
+                    } else {
+                        lowerBoundsMultipliers(iDynTreeIndex) = 0.0;
+                        upperBoundsMultipliers(iDynTreeIndex) = m_pimpl->eigenDualVariables(i);
+                    }
+                }
+            }
+            return true;
+        }
+
+        bool OsqpInterface::getOptimalCost(double &optimalCost)
+        {
+            if (!(m_pimpl->alreadySolved)) {
+                reportError("OsqpInterface", "getOptimalCost", "First you have to call the method solve once.");
+                return false;
+            }
+
+            if (!(m_problem->setVariables(m_pimpl->solutionBuffer))) {
+                reportError("OsqpInterface", "getOptimalCost", "Error while setting to optimization variables to the problem.");
+                return false;
+            }
+
+            if (!(m_problem->evaluateCostFunction(optimalCost))) {
+                reportError("OsqpInterface", "solve", "Error while evaluating cost function.");
+                return false;
+            }
+
+            return true;
+        }
+
+        bool OsqpInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
+        {
+            if (!(m_pimpl->alreadySolved)) {
+                reportError("OsqpInterface", "getOptimalConstraintsValues", "First you have to call the method solve once.");
+                return false;
+            }
+
+            if (!(m_problem->setVariables(m_pimpl->solutionBuffer))) {
+                reportError("OsqpInterface", "getOptimalConstraintsValues", "Error while setting to optimization variables to the problem.");
+                return false;
+            }
+
+            if (!(m_problem->evaluateConstraints(constraintsValues))) {
+                reportError("OsqpInterface", "solve", "Error while evaluating cost function.");
+                return false;
+            }
+
+            return true;
+        }
+
+        double OsqpInterface::minusInfinity()
+        {
+            return -OsqpEigen::INFTY;
+        }
+
+        double OsqpInterface::plusInfinity()
+        {
+            return OsqpEigen::INFTY;
+        }
+
+        const OsqpSettings &OsqpInterface::settings() const
+        {
+            return m_pimpl->settings;
+        }
+
+    }
+}
+
+

From 9bdda5e22f62817b95f1a4ff3b3f725eed190586 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 12 Jun 2018 10:29:57 +0200
Subject: [PATCH 029/194] Added test for a linear OC problem with OSQP. Some
 bugs to be solved.

Some other already solved.
---
 .../include/iDynTree/OptimalControlProblem.h  |  22 ++-
 .../iDynTree/Optimizers/OsqpInterface.h       |   2 +-
 .../src/OptimalControlProblem.cpp             |  45 ++++++-
 src/optimalcontrol/src/OsqpInterface.cpp      |  34 +++--
 src/optimalcontrol/src/QuadraticLikeCost.cpp  |   2 -
 src/optimalcontrol/tests/CMakeLists.txt       |   6 +
 src/optimalcontrol/tests/LinearOCOsqpTest.cpp | 127 ++++++++++++++++++
 .../tests/OptimalControlIpoptTest.cpp         |   3 -
 8 files changed, 216 insertions(+), 25 deletions(-)
 create mode 100644 src/optimalcontrol/tests/LinearOCOsqpTest.cpp

diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index 5161d6a1ac8..06622bfe497 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -36,7 +36,8 @@ namespace iDynTree {
         class LinearConstraint;
         class ConstraintsGroup;
         class Cost;
-        class QuadraticLikeCost;
+        class QuadraticCost;
+        class L2NormCost;
         class LinearCost;
         class TimeRange;
 
@@ -102,13 +103,17 @@ namespace iDynTree {
             // Lagrange term
             bool addMayerTerm(double weight, std::shared_ptr<Cost> cost); // final cost
 
-            bool addMayerTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost); // final cost
+            bool addMayerTerm(double weight, std::shared_ptr<QuadraticCost> quadraticCost); // final cost
+
+            bool addMayerTerm(double weight, std::shared_ptr<L2NormCost> quadraticCost); // final cost
 
             bool addMayerTerm(double weight, std::shared_ptr<LinearCost> linearCost); // final cost
 
             bool addLagrangeTerm(double weight, std::shared_ptr<Cost> cost); // integral cost
 
-            bool addLagrangeTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost); // integral cost
+            bool addLagrangeTerm(double weight, std::shared_ptr<QuadraticCost> quadraticCost); // integral cost
+
+            bool addLagrangeTerm(double weight, std::shared_ptr<L2NormCost> quadraticCost); // integral cost
 
             bool addLagrangeTerm(double weight, std::shared_ptr<LinearCost> linearCost); // integral cost
 
@@ -120,7 +125,12 @@ namespace iDynTree {
             bool addLagrangeTerm(double weight,
                                  double startingTime,
                                  double finalTime,
-                                 std::shared_ptr<QuadraticLikeCost> quadraticCost); // integral cost with explicit integration limits
+                                 std::shared_ptr<QuadraticCost> quadraticCost); // integral cost with explicit integration limits
+
+            bool addLagrangeTerm(double weight,
+                                 double startingTime,
+                                 double finalTime,
+                                 std::shared_ptr<L2NormCost> quadraticCost); // integral cost with explicit integration limits
 
             bool addLagrangeTerm(double weight,
                                  double startingTime,
@@ -129,7 +139,9 @@ namespace iDynTree {
 
             bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<Cost> cost);
 
-            bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<QuadraticLikeCost> quadraticCost);
+            bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<QuadraticCost> quadraticCost);
+
+            bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<L2NormCost> quadraticCost);
 
             bool addLagrangeTerm(double weight, const TimeRange& timeRange, std::shared_ptr<LinearCost> linearCost);
 
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
index c309534ffe8..feb7b024bc7 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
@@ -90,7 +90,7 @@ namespace iDynTree {
 
             virtual double plusInfinity() override;
 
-            const OsqpSettings& settings() const;
+            OsqpSettings& settings();
         };
 
     }
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 7a4cd0b9f15..e3bd13ae887 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -22,6 +22,8 @@
 #include <iDynTree/LinearConstraint.h>
 #include <iDynTree/Cost.h>
 #include <iDynTree/QuadraticLikeCost.h>
+#include <iDynTree/QuadraticCost.h>
+#include <iDynTree/L2NormCost.h>
 #include <iDynTree/LinearCost.h>
 #include <iDynTree/TimeRange.h>
 #include <iDynTree/Core/VectorDynSize.h>
@@ -420,7 +422,18 @@ namespace iDynTree {
             return true;
         }
 
-        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost)
+        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<QuadraticCost> quadraticCost)
+        {
+            TimeRange newTimerange(m_pimpl->horizon.endTime(), m_pimpl->horizon.endTime());
+            if (!(m_pimpl->addCost(weight, newTimerange, quadraticCost, false, true, "addMayerTerm"))) {
+                return false;
+            }
+            m_pimpl->mayerCostnames.push_back(quadraticCost->name());
+
+            return true;
+        }
+
+        bool OptimalControlProblem::addMayerTerm(double weight, std::shared_ptr<L2NormCost> quadraticCost)
         {
             TimeRange newTimerange(m_pimpl->horizon.endTime(), m_pimpl->horizon.endTime());
             if (!(m_pimpl->addCost(weight, newTimerange, quadraticCost, false, true, "addMayerTerm"))) {
@@ -447,7 +460,12 @@ namespace iDynTree {
             return (m_pimpl->addCost(weight, TimeRange::AnyTime(), cost, false, false, "addLagrangeTerm"));
         }
 
-        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<QuadraticLikeCost> quadraticCost)
+        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<QuadraticCost> quadraticCost)
+        {
+            return (m_pimpl->addCost(weight, TimeRange::AnyTime(), quadraticCost, false, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, std::shared_ptr<L2NormCost> quadraticCost)
         {
             return (m_pimpl->addCost(weight, TimeRange::AnyTime(), quadraticCost, false, true, "addLagrangeTerm"));
         }
@@ -471,7 +489,21 @@ namespace iDynTree {
             return (m_pimpl->addCost(weight, timeRange, cost, false, false, "addLagrangeTerm"));
         }
 
-        bool OptimalControlProblem::addLagrangeTerm(double weight, double startingTime, double finalTime, std::shared_ptr<QuadraticLikeCost> quadraticCost)
+        bool OptimalControlProblem::addLagrangeTerm(double weight, double startingTime, double finalTime, std::shared_ptr<QuadraticCost> quadraticCost)
+        {
+            TimeRange timeRange;
+
+            if (!timeRange.setTimeInterval(startingTime, finalTime)){
+                std::ostringstream errorMsg;
+                errorMsg << "The cost named " << quadraticCost->name() <<" has invalid time settings.";
+                reportError("OptimalControlProblem", "addLagrangeTerm", errorMsg.str().c_str());
+                return false;
+            }
+
+            return (m_pimpl->addCost(weight, timeRange, quadraticCost, false, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, double startingTime, double finalTime, std::shared_ptr<L2NormCost> quadraticCost)
         {
             TimeRange timeRange;
 
@@ -504,7 +536,12 @@ namespace iDynTree {
             return (m_pimpl->addCost(weight, timeRange, cost, false, false, "addLagrangeTerm"));
         }
 
-        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<QuadraticLikeCost> quadraticCost)
+        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<QuadraticCost> quadraticCost)
+        {
+            return (m_pimpl->addCost(weight, timeRange, quadraticCost, false, true, "addLagrangeTerm"));
+        }
+
+        bool OptimalControlProblem::addLagrangeTerm(double weight, const TimeRange &timeRange, std::shared_ptr<L2NormCost> quadraticCost)
         {
             return (m_pimpl->addCost(weight, timeRange, quadraticCost, false, true, "addLagrangeTerm"));
         }
diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index 575957387dc..784357d2aae 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -83,12 +83,20 @@ namespace iDynTree {
                 return *this;
             }
 
-            bool  operator==(const TripletIterator& rhs) const{
-                return ((rhs.m_nnzIndex == this->m_nnzIndex) && (rhs.m_nnzIdentity == this->m_nnzIdentity));
+            bool  operator==(const TripletIterator& rhs) const {
+                if (m_addIdentity) {
+                    return ((rhs.m_nnzIndex == this->m_nnzIndex) && (rhs.m_nnzIdentity == this->m_nnzIdentity));
+                } else {
+                    return (rhs.m_nnzIndex == this->m_nnzIndex);
+                }
             }
 
             bool operator!=(const TripletIterator& rhs) const {
-                return ((rhs.m_nnzIndex != this->m_nnzIndex) || (rhs.m_nnzIdentity != this->m_nnzIdentity));
+                if (m_addIdentity) {
+                    return ((rhs.m_nnzIndex != this->m_nnzIndex) || (rhs.m_nnzIdentity != this->m_nnzIdentity));
+                } else {
+                    return (rhs.m_nnzIndex != this->m_nnzIndex);
+                }
             }
 
             Triplet* operator->() {
@@ -104,6 +112,7 @@ namespace iDynTree {
                 }
 
                 assert(m_nnzIndex < m_colIndeces->size());
+
                 m_triplet.m_row = m_rowIndeces->operator[](m_nnzIndex);
                 m_triplet.m_col = m_colIndeces->operator[](m_nnzIndex);
                 unsigned int row = static_cast<unsigned int>(m_triplet.m_row);
@@ -117,6 +126,7 @@ namespace iDynTree {
                                          std::shared_ptr<MatrixDynSize> denseMatrix,
                                          bool addIdentityOnTop = false) {
                 TripletIterator m_begin(rowIndeces, colIndeces, denseMatrix, addIdentityOnTop);
+                assert(rowIndeces->size() == colIndeces->size());
                 return m_begin;
             }
 
@@ -166,7 +176,11 @@ namespace iDynTree {
             }
 
             bool  operator==(const DenseIterator& rhs) const{
-                return ((rhs.m_rowIndex == this->m_rowIndex) && (rhs.m_colIndex == this->m_colIndex) && (rhs.m_nnzIdentity == this->m_nnzIdentity));
+                if (m_addIdentity) {
+                    return ((rhs.m_rowIndex == this->m_rowIndex) && (rhs.m_colIndex == this->m_colIndex) && (rhs.m_nnzIdentity == this->m_nnzIdentity));
+                } else {
+                    return ((rhs.m_rowIndex == this->m_rowIndex) && (rhs.m_colIndex == this->m_colIndex));
+                }
             }
 
             bool operator!=(const DenseIterator& rhs) const {
@@ -444,6 +458,11 @@ namespace iDynTree {
                 return false;
             }
 
+            if (!m_problem->prepare()){
+                reportError("OsqpInterface", "solve", "Error while preparing the optimization problem.");
+                return false;
+            }
+
             if (!(m_problem->info().costIsQuadratic()) || (m_problem->info().hasNonLinearConstraints())) {
                 reportError("OsqpInterface", "solve", "The specified optimization problem cannot be solved by this solver. It needs to be a QP.");
                 return false;
@@ -454,11 +473,6 @@ namespace iDynTree {
                 return false;
             }
 
-            if (!m_problem->prepare()){
-                reportError("OsqpInterface", "solve", "Error while preparing the optimization problem.");
-                return false;
-            }
-
             m_pimpl->nv = m_problem->numberOfVariables();
 
             m_pimpl->isLowerBounded = m_problem->getVariablesLowerBound(m_pimpl->variablesLowerBounds);
@@ -833,7 +847,7 @@ namespace iDynTree {
             return OsqpEigen::INFTY;
         }
 
-        const OsqpSettings &OsqpInterface::settings() const
+        OsqpSettings &OsqpInterface::settings()
         {
             return m_pimpl->settings;
         }
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
index 150291bf0ef..497f3333f54 100644
--- a/src/optimalcontrol/src/QuadraticLikeCost.cpp
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -351,8 +351,6 @@ namespace iDynTree {
             partialDerivative.resize(control.size(), control.size());
 
             if (m_timeVaryingControlHessian) {
-                reportError("QuadraticLikeCost", "costSecondPartialDerivativeWRTControl", "The control cost is activated but the timeVaryingControlHessian pointer is empty.");
-                return false;
 
                 bool isValid = false;
                 const MatrixDynSize &hessian = m_timeVaryingControlHessian->get(time, isValid);
diff --git a/src/optimalcontrol/tests/CMakeLists.txt b/src/optimalcontrol/tests/CMakeLists.txt
index f61701d99c9..428ea93866e 100644
--- a/src/optimalcontrol/tests/CMakeLists.txt
+++ b/src/optimalcontrol/tests/CMakeLists.txt
@@ -21,3 +21,9 @@ if (IDYNTREE_USES_IPOPT)
     add_oc_test(Optimizer)
     add_oc_test(OptimalControlIpopt)
 endif()
+
+if (OC_USE_OSQP)
+    add_oc_test(LinearOCOsqp)
+endif()
+
+
diff --git a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
new file mode 100644
index 00000000000..7012acbf68f
--- /dev/null
+++ b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/TestUtils.h>
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/LinearSystem.h>
+#include <iDynTree/L2NormCost.h>
+#include <iDynTree/LinearConstraint.h>
+#include <iDynTree/ConstraintsGroup.h>
+#include <iDynTree/TimeRange.h>
+#include <iDynTree/Integrators/ForwardEuler.h>
+#include <iDynTree/OCSolvers/MultipleShootingSolver.h>
+#include <iDynTree/Optimizers/OsqpInterface.h>
+#include <iDynTree/OptimalControlProblem.h>
+
+#include <vector>
+#include <ctime>
+
+using namespace iDynTree::optimalcontrol;
+using namespace iDynTree;
+
+int main() {
+    std::shared_ptr<LinearSystem> doubleIntegrator(new LinearSystem(2, 1));
+
+    MatrixDynSize stateMatrix(2,2), controlMatrix(2,1);
+    stateMatrix.zero();
+    stateMatrix(0,1) = 1.0;
+    controlMatrix.zero();
+    controlMatrix(1,0) = 1.0;
+
+    ASSERT_IS_TRUE(doubleIntegrator->setStateMatrix(stateMatrix));
+    ASSERT_IS_TRUE(doubleIntegrator->setControlMatrix(controlMatrix));
+
+    std::shared_ptr<OptimalControlProblem> problem(new OptimalControlProblem());
+    ASSERT_IS_TRUE(problem->setDynamicalSystemConstraint(doubleIntegrator));
+
+    std::shared_ptr<L2NormCost> quadraticCost(new L2NormCost("normCost", 2, 1));
+    ASSERT_IS_TRUE(problem->addLagrangeTerm(1.0, quadraticCost));
+
+    ASSERT_IS_TRUE(problem->setTimeHorizon(0.0, 1.0));
+
+    iDynTree::VectorDynSize bound(1);
+    bound(0) = 0.8;
+    ASSERT_IS_TRUE(problem->setControlUpperBound(bound));
+    bound(0) = -0.9;
+    ASSERT_IS_TRUE(problem->setControlLowerBound(bound));
+
+    std::shared_ptr<LinearConstraint> controlConstraint(new LinearConstraint(1, "linearConstraint"));
+    MatrixDynSize controlConstraintMatrix(1,1);
+    controlConstraintMatrix(0,0) = 1.0;
+    ASSERT_IS_TRUE(controlConstraint->setControlConstraintMatrix(controlConstraintMatrix));
+    iDynTree::VectorDynSize upperBound(1);
+    upperBound(0) = 0.5;
+    ASSERT_IS_TRUE(controlConstraint->setUpperBound(upperBound));
+
+    std::shared_ptr<ConstraintsGroup> group(new ConstraintsGroup("group", 1));
+    ASSERT_IS_TRUE(group->addConstraint(controlConstraint, iDynTree::optimalcontrol::TimeRange(0.6, 1.0)));
+    ASSERT_IS_TRUE(problem->addGroupOfConstraints(group));
+
+    MultipleShootingSolver solver(problem);
+
+
+    std::shared_ptr<integrators::ForwardEuler> integrator(new integrators::ForwardEuler);
+    ASSERT_IS_TRUE(solver.setIntegrator(integrator));
+    std::shared_ptr<optimization::OsqpInterface> optimizer(new optimization::OsqpInterface);
+    ASSERT_IS_TRUE(solver.setOptimizer(optimizer));
+    ASSERT_IS_TRUE(solver.setStepSizeBounds(0.001, 0.01));
+    ASSERT_IS_TRUE(solver.setControlPeriod(0.01));
+
+    iDynTree::VectorDynSize initialState(2);
+    iDynTree::getRandomVector(initialState, -2.0, 2.0);
+    ASSERT_IS_TRUE(solver.setInitialState(initialState));
+    optimizer->settings().verbose = true;
+
+    clock_t initT, endT;
+    initT = clock();
+    ASSERT_IS_TRUE(solver.solve());
+    endT = clock();
+
+    std::vector<iDynTree::VectorDynSize> states, controls;
+    ASSERT_IS_TRUE(solver.getSolution(states, controls));
+    std::cerr << "Initial state: " << initialState.toString() << std::endl;
+    std::cerr << "First state: " << states.front().toString() << std::endl;
+    std::cerr << "Last state: " << states.back().toString() << std::endl;
+    std::cerr << "First control: " << controls.front().toString() << std::endl;
+    std::cerr << "Last control: " << controls.back().toString() << std::endl;
+    std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+
+    for (int i=0; i < 2; ++i){
+
+        std::cerr << "------------" << std::endl;
+
+        //iDynTree::getRandomVector(initialState, -2.0, 2.0);
+        initialState = states.front();
+        ASSERT_IS_TRUE(solver.setInitialState(initialState));
+
+        initT = clock();
+        ASSERT_IS_TRUE(solver.solve());
+        endT = clock();
+
+
+        ASSERT_IS_TRUE(solver.getSolution(states, controls));
+        std::cerr << "Initial state: " << initialState.toString() << std::endl;
+        std::cerr << "First state: " << states.front().toString() << std::endl;
+        std::cerr << "Last state: " << states.back().toString() << std::endl;
+        std::cerr << "First control: " << controls.front().toString() << std::endl;
+        std::cerr << "Last control: " << controls.back().toString() << std::endl;
+        std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+    }
+
+    return EXIT_SUCCESS;
+}
diff --git a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
index 6e2e65be434..8e80a564329 100644
--- a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
+++ b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
@@ -257,9 +257,6 @@ int main () {
     // Optimizer settings
     //ASSERT_IS_TRUE(optimizer->setIpoptOption("linear_solver", "ma27"));
     ASSERT_IS_TRUE(optimizer->setIpoptOption("print_level", 0));
-    ASSERT_IS_TRUE(optimizer->setIpoptOption("hessian_constant", "yes"));
-    ASSERT_IS_TRUE(optimizer->setIpoptOption("jac_c_constant", "yes"));
-    ASSERT_IS_TRUE(optimizer->setIpoptOption("jac_d_constant", "yes"));
 
     clock_t initT, endT;
     initT = clock();

From e0a12418c9a71eea60bc26acc6b8666eb501d476 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 12 Jun 2018 12:45:55 +0200
Subject: [PATCH 030/194] Fixed bugs when setting mesh points.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index bfef86e448e..2e1f37690c4 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -124,7 +124,7 @@ namespace iDynTree {
                 }
             }
 
-            std::vector<MeshPoint>::iterator findNextMeshPoint(std::vector<MeshPoint>::iterator& start){
+            std::vector<MeshPoint>::iterator findNextMeshPoint(const std::vector<MeshPoint>::iterator& start){
                 std::vector<MeshPoint>::iterator nextMesh = start;
                 MeshPointOrigin ignored = MeshPointOrigin::Ignored();
                 assert(nextMesh != m_meshPoints.end());
@@ -318,6 +318,7 @@ namespace iDynTree {
                 if ((lastControlMeshIterator->origin == MeshPointOrigin::LastPoint()) && (m_integrator->info().isExplicit())) {//the last control input would have no effect
                     controlMeshes--;
                     setIgnoredMesh(lastControlMeshIterator);
+                    lastControlMeshIterator--;
                 }
 
                 newMeshPoint.origin = MeshPointOrigin::LastPoint();
@@ -517,14 +518,17 @@ namespace iDynTree {
 
                     nextMesh = findNextMeshPoint(mesh); //find next valid mesh
                     timeDistance = std::abs(nextMesh->time - mesh->time); //for the way I have ordered the vector, it can be negative
+                    assert(timeDistance < (endTime - initTime));
 
                     if (timeDistance > m_maxStepSize){ //two consecutive points are too distant
                         unsigned int additionalPoints = static_cast<unsigned int>(std::ceil(timeDistance/(m_maxStepSize))) - 1;
                         double dtAdd = timeDistance / (additionalPoints + 1); //additionalPoints + 1 is the number of segments.
                         long nextPosition = nextMesh - m_meshPoints.begin();
                         //since tmin < tmax/2, dtAdd > tmin. Infact, the worst case is when timeDistance is nearly equal to tmax.
+                        double startTime = std::min(mesh->time, nextMesh->time);
                         for (unsigned int i = 0; i < additionalPoints; ++i) {
-                            newMeshPoint.time = mesh->time + (i + 1)*dtAdd;
+                            newMeshPoint.time = startTime + (i + 1)*dtAdd;
+                            assert((newMeshPoint.time >= initTime) && (newMeshPoint.time <= endTime));
                             addMeshPoint(newMeshPoint);
                         }
                         nextMesh = m_meshPoints.begin() + nextPosition;
@@ -606,6 +610,7 @@ namespace iDynTree {
                                 newMeshPoint.origin = MeshPointOrigin::FillVariables();
                                 newMeshPoint.type = MeshPointType::State;
                                 while ((mesh->origin != last) && (toBeAdded > 0)){
+                                    toBeAdded = static_cast<unsigned int>(m_totalMeshes - newTotalMeshes);
                                     nextMesh = findNextMeshPoint(mesh); //find next valid mesh
                                     timeDistance = std::abs(nextMesh->time - mesh->time);
 
@@ -614,8 +619,9 @@ namespace iDynTree {
                                         unsigned int meshToAddHere = std::min(toBeAdded, possibleMeshes);
                                         double dT = timeDistance/(meshToAddHere + 1);
                                         long nextPosition = nextMesh - m_meshPoints.begin();
+                                        double startTime = std::min(mesh->time, nextMesh->time);
                                         for(unsigned int m = 1; m <= meshToAddHere; m++){
-                                            newMeshPoint.time = mesh->time + m*dT;
+                                            newMeshPoint.time = startTime + m*dT;
                                             addMeshPoint(newMeshPoint);
                                             newTotalMeshes++;
                                         }

From 93cdd290bff691613e4a8ec3ffbc924add3d04e2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 12 Jun 2018 16:32:43 +0200
Subject: [PATCH 031/194] The initial state is now part of the optimiztion
 variables.

This make possible to use QP solvers, since the constant part of the constraints would be lost otherwise. Solved a bug when adding an identity on top of the constraints jacobian in the OSQP interface.
---
 .../src/MultipleShootingSolver.cpp            | 239 +++++++++---------
 src/optimalcontrol/src/OsqpInterface.cpp      |  21 +-
 src/optimalcontrol/tests/LinearOCOsqpTest.cpp |   9 +-
 .../tests/OptimalControlIpoptTest.cpp         |  20 +-
 4 files changed, 151 insertions(+), 138 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 2e1f37690c4..2200f8591e9 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -183,6 +183,14 @@ namespace iDynTree {
                 }
             }
 
+            void addIdentityJacobianBlock(size_t initRow, size_t initCol, size_t dimension){
+                for (size_t i = 0; i < dimension; ++i){
+                    addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + i);
+                    addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + i);
+                    m_jacobianNonZeros++;
+                }
+            }
+
             void addHessianBlock(size_t initRow, size_t rows, size_t initCol, size_t cols){
                 for (size_t i = 0; i < rows; ++i){
                     for (size_t j = 0; j < cols; ++j){
@@ -833,42 +841,42 @@ namespace iDynTree {
 
             MultipleShootingTranscription()
             : m_ocproblem(nullptr)
-            ,m_integrator(nullptr)
-            ,m_totalMeshes(0)
-            ,m_controlMeshes(0)
-            ,m_prepared(false)
-            ,m_meshPointsEnd(m_meshPoints.end())
-            ,m_minStepSize(0.001)
-            ,m_maxStepSize(0.01)
-            ,m_controlPeriod(0.01)
-            ,m_nx(0)
-            ,m_nu(0)
-            ,m_numberOfVariables(0)
-            ,m_constraintsPerInstant(0)
-            ,m_numberOfConstraints(0)
-            ,m_plusInfinity(1e19)
-            ,m_minusInfinity(-1e19)
-            ,m_solved(false)
+            , m_integrator(nullptr)
+            , m_totalMeshes(0)
+            , m_controlMeshes(0)
+            , m_prepared(false)
+            , m_meshPointsEnd(m_meshPoints.end())
+            , m_minStepSize(0.001)
+            , m_maxStepSize(0.01)
+            , m_controlPeriod(0.01)
+            , m_nx(0)
+            , m_nu(0)
+            , m_numberOfVariables(0)
+            , m_constraintsPerInstant(0)
+            , m_numberOfConstraints(0)
+            , m_plusInfinity(1e19)
+            , m_minusInfinity(-1e19)
+            , m_solved(false)
             { }
 
             MultipleShootingTranscription(const std::shared_ptr<OptimalControlProblem> problem, const std::shared_ptr<Integrator> integrationMethod)
-            :m_ocproblem(problem)
-            ,m_integrator(integrationMethod)
-            ,m_totalMeshes(0)
-            ,m_controlMeshes(0)
-            ,m_prepared(false)
-            ,m_meshPointsEnd(m_meshPoints.end())
-            ,m_minStepSize(0.001)
-            ,m_maxStepSize(0.01)
-            ,m_controlPeriod(0.01)
-            ,m_nx(0)
-            ,m_nu(0)
-            ,m_numberOfVariables(0)
-            ,m_constraintsPerInstant(0)
-            ,m_numberOfConstraints(0)
-            ,m_plusInfinity(1e19)
-            ,m_minusInfinity(-1e19)
-            ,m_solved(false)
+            : m_ocproblem(problem)
+            , m_integrator(integrationMethod)
+            , m_totalMeshes(0)
+            , m_controlMeshes(0)
+            , m_prepared(false)
+            , m_meshPointsEnd(m_meshPoints.end())
+            , m_minStepSize(0.001)
+            , m_maxStepSize(0.01)
+            , m_controlPeriod(0.01)
+            , m_nx(0)
+            , m_nu(0)
+            , m_numberOfVariables(0)
+            , m_constraintsPerInstant(0)
+            , m_numberOfConstraints(0)
+            , m_plusInfinity(1e19)
+            , m_minusInfinity(-1e19)
+            , m_solved(false)
             { }
 
             MultipleShootingTranscription(const MultipleShootingTranscription& other) = delete;
@@ -892,11 +900,11 @@ namespace iDynTree {
                 m_nu = nu;
 
                 //TODO: I should consider also the possibility to have auxiliary variables in the integrator
-                m_numberOfVariables = (m_totalMeshes - 1) * nx + m_controlMeshes * nu; //the -1 removes the initial state from the set of optimization varibales
+                m_numberOfVariables = (m_totalMeshes) * nx + m_controlMeshes * nu; //also the initial state should be an optimization variable. This allows to use only the constraint jacobian in case of a linear problem
 
                 m_constraintsPerInstant = m_ocproblem->getConstraintsDimension();
                 size_t nc = m_constraintsPerInstant;
-                m_numberOfConstraints = (m_totalMeshes - 1) * nx + (m_constraintsPerInstant) * (m_totalMeshes); //dynamical constraints (removing the initial state) and normal constraints
+                m_numberOfConstraints = (m_totalMeshes) * nx + (m_constraintsPerInstant) * (m_totalMeshes); //dynamical constraints (plus identity constraint for the initial state) and normal constraints
 
                 //Determine problem type
                 m_infoData->hasLinearConstraints = (m_ocproblem->countLinearConstraints() != 0) || m_ocproblem->systemIsLinear();
@@ -921,11 +929,18 @@ namespace iDynTree {
                 while (mesh != m_meshPointsEnd){
                     if (mesh->origin == first){
                         //setting up the indeces
+                        mesh->stateIndex = index; //the initial state goes first
+                        index += nx;
                         mesh->controlIndex = index;
                         mesh->previousControlIndex = index;
                         index += nu;
                         previousControlMesh = mesh;
 
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)) = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)) = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+                        addIdentityJacobianBlock(constraintIndex, mesh->stateIndex, nx);
+                        constraintIndex += nx;
+
                         //Saving constraints bounds
                         if (!(m_ocproblem->getConstraintsLowerBound(mesh->time, m_minusInfinity, m_constraintsBuffer))){
                             std::ostringstream errorMsg;
@@ -945,9 +960,17 @@ namespace iDynTree {
                         addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         constraintIndex += nc;
 
+                        //Saving the hessian structure
                         //Saving the hessian structure
                         if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
                             addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+                            addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
+
+                            addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
+                            addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+
+                            addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
+                            addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
                         }
 
                     } else if (mesh->type == MeshPointType::Control) {
@@ -966,9 +989,8 @@ namespace iDynTree {
                         addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
                         addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
                         addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
-                        if ((mesh - 1)->origin != first){
-                            addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
-                        }
+                        addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+
                         constraintIndex += nx;
 
                         //Saving constraints bounds
@@ -1004,10 +1026,8 @@ namespace iDynTree {
                         }
 
                         if (!(m_ocproblem->systemIsLinear())) {
-                            if ((mesh - 1)->origin != first){
-                                addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
-                                addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
-                            }
+                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
                         }
 
 
@@ -1024,9 +1044,7 @@ namespace iDynTree {
                         //Saving the jacobian structure due to the dynamical constraints
                         addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
                         addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
-                        if ((mesh - 1)->origin != first){
-                            addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
-                        }
+                        addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
                         constraintIndex += nx;
 
                         //Saving constraints bounds
@@ -1058,10 +1076,8 @@ namespace iDynTree {
                         }
 
                         if (!(m_ocproblem->systemIsLinear())) {
-                            if ((mesh - 1)->origin != first){
-                                addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
-                                addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
-                            }
+                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
                         }
 
                     }
@@ -1143,6 +1159,7 @@ namespace iDynTree {
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
+                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_plusInfinity); //avoiding setting bounds on the initial state
                         upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
                     } else if (mesh->type == MeshPointType::Control) {
                         upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
@@ -1186,6 +1203,7 @@ namespace iDynTree {
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_minusInfinity); //avoiding setting bounds on the initial state
                         lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
                     } else if (mesh->type == MeshPointType::Control) {
                         lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
@@ -1269,13 +1287,9 @@ namespace iDynTree {
                 costValue = 0;
                 double singleCost;
 
-                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
-                    if (mesh->origin == first){
-                        stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                    } else {
-                        stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
-                    }
+
+                    stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                     controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
                     if (!(m_ocproblem->costsEvaluation(mesh->time, m_stateBuffer, m_controlBuffer, singleCost))){
@@ -1313,26 +1327,21 @@ namespace iDynTree {
 
                 Eigen::Map<Eigen::VectorXd> gradientMap = toEigen(gradient);
 
-                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
-                    if (mesh->origin == first){
-                        stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                    } else {
-                        stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
-                    }
-                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
-                    if (mesh->origin != first){
-                        if (!(m_ocproblem->costsFirstPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costStateGradientBuffer))){
-                            std::ostringstream errorMsg;
-                            errorMsg << "Error while evaluating cost state gradient at time t = " << mesh->time << ".";
-                            reportError("MultipleShootingTranscription", "evaluateCostGradient", errorMsg.str().c_str());
-                            return false;
-                        }
+                    stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
+                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
-                        gradientMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = costStateGradient;
+                    if (!(m_ocproblem->costsFirstPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costStateGradientBuffer))){
+                        std::ostringstream errorMsg;
+                        errorMsg << "Error while evaluating cost state gradient at time t = " << mesh->time << ".";
+                        reportError("MultipleShootingTranscription", "evaluateCostGradient", errorMsg.str().c_str());
+                        return false;
                     }
 
+                    gradientMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = costStateGradient;
+
+
                     if (!(m_ocproblem->costsFirstPartialDerivativeWRTControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costControlGradientBuffer))){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost control gradient at time t = " << mesh->time << ".";
@@ -1376,37 +1385,31 @@ namespace iDynTree {
 
                 iDynTreeEigenMatrixMap hessianMap = toEigen(hessian);
 
-                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
-                    if (mesh->origin == first){
-                        stateBufferMap = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                    } else {
-                        stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
-                    }
-                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
-                    if (mesh->origin != first){
-                        if (!(m_ocproblem->costsSecondPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateBuffer))){
-                            std::ostringstream errorMsg;
-                            errorMsg << "Error while evaluating cost state hessian at time t = " << mesh->time << ".";
-                            reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
-                            return false;
-                        }
+                    stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
+                    controlBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
 
-                        hessianMap.block(static_cast<Eigen::Index>(mesh->stateIndex), static_cast<Eigen::Index>(mesh->stateIndex), nx, nx) = costStateHessian;
+                    if (!(m_ocproblem->costsSecondPartialDerivativeWRTState(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateBuffer))){
+                        std::ostringstream errorMsg;
+                        errorMsg << "Error while evaluating cost state hessian at time t = " << mesh->time << ".";
+                        reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
+                        return false;
+                    }
 
-                        if (!(m_ocproblem->costsSecondPartialDerivativeWRTStateControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateControlBuffer))){
-                            std::ostringstream errorMsg;
-                            errorMsg << "Error while evaluating cost state-control hessian at time t = " << mesh->time << ".";
-                            reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
-                            return false;
-                        }
+                    hessianMap.block(static_cast<Eigen::Index>(mesh->stateIndex), static_cast<Eigen::Index>(mesh->stateIndex), nx, nx) = costStateHessian;
 
-                        hessianMap.block(static_cast<Eigen::Index>(mesh->stateIndex), static_cast<Eigen::Index>(mesh->controlIndex), nx, nu) = costStateControlHessian;
-                        costControlStateHessian = costStateControlHessian.transpose();
-                        hessianMap.block(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(mesh->stateIndex), nu, nx) = costControlStateHessian;
+                    if (!(m_ocproblem->costsSecondPartialDerivativeWRTStateControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianStateControlBuffer))){
+                        std::ostringstream errorMsg;
+                        errorMsg << "Error while evaluating cost state-control hessian at time t = " << mesh->time << ".";
+                        reportError("MultipleShootingTranscription", "evaluateCostHessian", errorMsg.str().c_str());
+                        return false;
                     }
 
+                    hessianMap.block(static_cast<Eigen::Index>(mesh->stateIndex), static_cast<Eigen::Index>(mesh->controlIndex), nx, nu) = costStateControlHessian;
+                    costControlStateHessian = costStateControlHessian.transpose();
+                    hessianMap.block(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(mesh->stateIndex), nu, nx) = costControlStateHessian;
+
                     if (!(m_ocproblem->costsSecondPartialDerivativeWRTControl(mesh->time, m_stateBuffer, m_controlBuffer, m_costHessianControlBuffer))){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost control hessian at time t = " << mesh->time << ".";
@@ -1453,20 +1456,17 @@ namespace iDynTree {
                 Eigen::Index constraintIndex = 0;
                 double dT = 0;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
-                    if (mesh->origin == first){
-                         currentState= toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                    } else {
-                        currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
-                        if ((mesh -1)->origin == first){
-                            previousState = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                        } else {
-                            previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
-                        }
-                    }
+
+                    currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
+
                     currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
                     previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
 
-                    if (mesh->origin != first){
+                    if (mesh->origin == first) {
+                        constraintsMap.segment(constraintIndex, nx) = currentState; //identity constraint for the initial state
+                        constraintIndex += nx;
+                    } else {
+                        previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
                         dT = mesh->time - (mesh - 1)->time;
                         if (!(m_integrator->evaluateCollocationConstraint(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT, m_stateBuffer))){
                             std::ostringstream errorMsg;
@@ -1519,20 +1519,18 @@ namespace iDynTree {
                 Eigen::Index constraintIndex = 0;
                 double dT = 0;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
-                    if (mesh->origin == first){
-                         currentState= toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                    } else {
-                        currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
-                        if ((mesh -1)->origin == first){
-                            previousState = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                        } else {
-                            previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
-                        }
-                    }
+
+                    currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
                     currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
                     previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
 
-                    if (mesh->origin != first){
+                    if (mesh->origin == first) {
+
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nx, nx).setIdentity();
+                        constraintIndex += nx;
+
+                    } else {
+                        previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
                         dT = mesh->time - (mesh - 1)->time;
                         if (!(m_integrator->evaluateCollocationConstraintJacobian(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT, m_collocationStateJacBuffer, m_collocationControlJacBuffer))){
                             std::ostringstream errorMsg;
@@ -1541,9 +1539,8 @@ namespace iDynTree {
                             return false;
                         }
 
-                        if ((mesh -1)->origin != first) {
-                            jacobianMap.block(constraintIndex, static_cast<Eigen::Index>((mesh-1)->stateIndex), nx, nx) = toEigen(m_collocationStateJacBuffer[0]);
-                        }
+
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>((mesh-1)->stateIndex), nx, nx) = toEigen(m_collocationStateJacBuffer[0]);
 
                         jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nx, nx) = toEigen(m_collocationStateJacBuffer[1]);
 
@@ -1564,9 +1561,7 @@ namespace iDynTree {
                         return false;
                     }
 
-                    if (mesh->origin != first) {
-                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nc, nx) = toEigen(m_constraintsStateJacBuffer);
-                    }
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nc, nx) = toEigen(m_constraintsStateJacBuffer);                    
 
                     if (!(m_ocproblem->constraintsJacobianWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsControlJacBuffer))){
                         std::ostringstream errorMsg;
diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index 784357d2aae..9d63f01e030 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -113,9 +113,9 @@ namespace iDynTree {
 
                 assert(m_nnzIndex < m_colIndeces->size());
 
-                m_triplet.m_row = m_rowIndeces->operator[](m_nnzIndex);
+                m_triplet.m_row = m_rowIndeces->operator[](m_nnzIndex) + m_nnzIdentity;
                 m_triplet.m_col = m_colIndeces->operator[](m_nnzIndex);
-                unsigned int row = static_cast<unsigned int>(m_triplet.m_row);
+                unsigned int row = static_cast<unsigned int>(m_rowIndeces->operator[](m_nnzIndex));
                 unsigned int col = static_cast<unsigned int>(m_triplet.m_col);
                 m_triplet.m_value = m_denseMatrix->operator()(row, col);
                 return m_triplet;
@@ -135,7 +135,9 @@ namespace iDynTree {
                                        std::shared_ptr<MatrixDynSize> denseMatrix,
                                        bool addIdentityOnTop = false) {
                 TripletIterator m_end(rowIndeces, colIndeces, denseMatrix, addIdentityOnTop);
-                m_end.m_nnzIdentity = denseMatrix->cols();
+                if (addIdentityOnTop) {
+                    m_end.m_nnzIdentity = denseMatrix->cols();
+                }
                 m_end.m_nnzIndex = rowIndeces->size();
                 return m_end;
             }
@@ -202,9 +204,9 @@ namespace iDynTree {
                 assert(m_rowIndex < m_denseMatrix->rows());
                 assert(m_colIndex < m_denseMatrix->cols());
 
-                m_triplet.m_row = m_rowIndex;
+                m_triplet.m_row = m_rowIndex + m_nnzIdentity;
                 m_triplet.m_col = m_colIndex;
-                unsigned int row = static_cast<unsigned int>(m_triplet.m_row);
+                unsigned int row = static_cast<unsigned int>(m_rowIndex);
                 unsigned int col = static_cast<unsigned int>(m_triplet.m_col);
                 m_triplet.m_value = m_denseMatrix->operator()(row, col);
                 return m_triplet;
@@ -219,7 +221,9 @@ namespace iDynTree {
             static DenseIterator end(std::shared_ptr<MatrixDynSize> denseMatrix,
                                      bool addIdentityOnTop = false) {
                 DenseIterator m_end(denseMatrix, addIdentityOnTop);
-                m_end.m_nnzIdentity = denseMatrix->cols();
+                if (addIdentityOnTop) {
+                    m_end.m_nnzIdentity = denseMatrix->cols();
+                }
                 m_end.m_rowIndex = denseMatrix->rows();
                 m_end.m_colIndex = denseMatrix->cols();
                 return m_end;
@@ -712,6 +716,11 @@ namespace iDynTree {
                 }
             }
 
+            if (!(m_pimpl->solver.isInitialized())) {
+                reportError("OsqpInterface", "solve", "The solver does not seem to be initialized correctly.");
+                return false;
+            }
+
             if (!(m_pimpl->solver.solve())) {
                 reportError("OsqpInterface", "solve", "Error while solving the QP.");
                 return false;
diff --git a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
index 7012acbf68f..3a5327e5cbe 100644
--- a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
+++ b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
@@ -26,6 +26,7 @@
 #include <iDynTree/Integrators/ForwardEuler.h>
 #include <iDynTree/OCSolvers/MultipleShootingSolver.h>
 #include <iDynTree/Optimizers/OsqpInterface.h>
+//#include <iDynTree/Optimizers/IpoptInterface.h>
 #include <iDynTree/OptimalControlProblem.h>
 
 #include <vector>
@@ -77,7 +78,14 @@ int main() {
 
     std::shared_ptr<integrators::ForwardEuler> integrator(new integrators::ForwardEuler);
     ASSERT_IS_TRUE(solver.setIntegrator(integrator));
+
+
     std::shared_ptr<optimization::OsqpInterface> optimizer(new optimization::OsqpInterface);
+    optimizer->settings().verbose = false;
+
+//    std::shared_ptr<optimization::IpoptInterface> optimizer(new optimization::IpoptInterface);
+//    ASSERT_IS_TRUE(optimizer->setIpoptOption("print_level", 0));
+
     ASSERT_IS_TRUE(solver.setOptimizer(optimizer));
     ASSERT_IS_TRUE(solver.setStepSizeBounds(0.001, 0.01));
     ASSERT_IS_TRUE(solver.setControlPeriod(0.01));
@@ -85,7 +93,6 @@ int main() {
     iDynTree::VectorDynSize initialState(2);
     iDynTree::getRandomVector(initialState, -2.0, 2.0);
     ASSERT_IS_TRUE(solver.setInitialState(initialState));
-    optimizer->settings().verbose = true;
 
     clock_t initT, endT;
     initT = clock();
diff --git a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
index 8e80a564329..f0a247fb04d 100644
--- a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
+++ b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
@@ -246,7 +246,7 @@ int main () {
     // Multiple Shooting settings
     ASSERT_IS_TRUE(solver.setIntegrator(integrator));
     ASSERT_IS_TRUE(solver.setOptimizer(optimizer));
-    ASSERT_IS_TRUE(solver.setStepSizeBounds(0.001, 0.01));
+    ASSERT_IS_TRUE(solver.setStepSizeBounds(0.001, 0.02));
     ASSERT_IS_TRUE(solver.setControlPeriod(0.01));
 
     iDynTree::VectorDynSize initialState(2);
@@ -272,13 +272,15 @@ int main () {
     std::cerr << "Last control: " << controls.back().toString() << std::endl;
     std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
 
-    for (int i=0; i < 2; ++i){
+    for (int i=0; i < 5; ++i){
 
-        std::cerr << "------------" << std::endl;
+//        std::cerr << "------------" << std::endl;
 
         //iDynTree::getRandomVector(initialState, -2.0, 2.0);
         initialState = states.front();
         ASSERT_IS_TRUE(solver.setInitialState(initialState));
+        ASSERT_IS_TRUE(problem->setTimeHorizon(0.0 + i*0.01, 1.0 + i*0.01));
+
 
         initT = clock();
         ASSERT_IS_TRUE(solver.solve());
@@ -286,12 +288,12 @@ int main () {
 
 
         ASSERT_IS_TRUE(solver.getSolution(states, controls));
-        std::cerr << "Initial state: " << initialState.toString() << std::endl;
-        std::cerr << "First state: " << states.front().toString() << std::endl;
-        std::cerr << "Last state: " << states.back().toString() << std::endl;
-        std::cerr << "First control: " << controls.front().toString() << std::endl;
-        std::cerr << "Last control: " << controls.back().toString() << std::endl;
-        std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
+//        std::cerr << "Initial state: " << initialState.toString() << std::endl;
+//        std::cerr << "First state: " << states.front().toString() << std::endl;
+//        std::cerr << "Last state: " << states.back().toString() << std::endl;
+//        std::cerr << "First control: " << controls.front().toString() << std::endl;
+//        std::cerr << "Last control: " << controls.back().toString() << std::endl;
+//        std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
     }
 
     return EXIT_SUCCESS;

From 4a3565d6ce20dd78ec04f0bb6334ef3dfe62e3a9 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 12 Jun 2018 18:31:43 +0200
Subject: [PATCH 032/194] Full clear of OSQP structures.

---
 src/optimalcontrol/src/OsqpInterface.cpp      |  2 ++
 src/optimalcontrol/tests/LinearOCOsqpTest.cpp | 26 ++++++++++++-------
 2 files changed, 18 insertions(+), 10 deletions(-)

diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index 9d63f01e030..9f684250f19 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -665,6 +665,8 @@ namespace iDynTree {
             } else {
                 if (m_pimpl->solver.isInitialized()) {
                     m_pimpl->solver.clearSolver();
+                    m_pimpl->solver.data()->clearHessianMatrix();
+                    m_pimpl->solver.data()->clearLinearConstraintsMatrix();
                 }
                 m_pimpl->alreadySolved = false;
 
diff --git a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
index 3a5327e5cbe..1b08d67b5d1 100644
--- a/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
+++ b/src/optimalcontrol/tests/LinearOCOsqpTest.cpp
@@ -53,7 +53,8 @@ int main() {
     std::shared_ptr<L2NormCost> quadraticCost(new L2NormCost("normCost", 2, 1));
     ASSERT_IS_TRUE(problem->addLagrangeTerm(1.0, quadraticCost));
 
-    ASSERT_IS_TRUE(problem->setTimeHorizon(0.0, 1.0));
+    double horizonLength = 1.0;
+    ASSERT_IS_TRUE(problem->setTimeHorizon(0.0, horizonLength));
 
     iDynTree::VectorDynSize bound(1);
     bound(0) = 0.8;
@@ -82,12 +83,13 @@ int main() {
 
     std::shared_ptr<optimization::OsqpInterface> optimizer(new optimization::OsqpInterface);
     optimizer->settings().verbose = false;
+    optimizer->settings().scaling = 0;
 
 //    std::shared_ptr<optimization::IpoptInterface> optimizer(new optimization::IpoptInterface);
 //    ASSERT_IS_TRUE(optimizer->setIpoptOption("print_level", 0));
 
     ASSERT_IS_TRUE(solver.setOptimizer(optimizer));
-    ASSERT_IS_TRUE(solver.setStepSizeBounds(0.001, 0.01));
+    ASSERT_IS_TRUE(solver.setStepSizeBounds(0.001, 0.011));
     ASSERT_IS_TRUE(solver.setControlPeriod(0.01));
 
     iDynTree::VectorDynSize initialState(2);
@@ -108,25 +110,29 @@ int main() {
     std::cerr << "Last control: " << controls.back().toString() << std::endl;
     std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
 
-    for (int i=0; i < 2; ++i){
+    for (int i=0; i < 5; ++i){
 
-        std::cerr << "------------" << std::endl;
+//        std::cerr << "------------" << std::endl;
 
         //iDynTree::getRandomVector(initialState, -2.0, 2.0);
         initialState = states.front();
         ASSERT_IS_TRUE(solver.setInitialState(initialState));
+        ASSERT_IS_TRUE(problem->setTimeHorizon(0.0 + i*0.01, horizonLength + i*0.01));
+
 
         initT = clock();
         ASSERT_IS_TRUE(solver.solve());
+        //solver.solve();
         endT = clock();
 
 
-        ASSERT_IS_TRUE(solver.getSolution(states, controls));
-        std::cerr << "Initial state: " << initialState.toString() << std::endl;
-        std::cerr << "First state: " << states.front().toString() << std::endl;
-        std::cerr << "Last state: " << states.back().toString() << std::endl;
-        std::cerr << "First control: " << controls.front().toString() << std::endl;
-        std::cerr << "Last control: " << controls.back().toString() << std::endl;
+        //ASSERT_IS_TRUE(solver.getSolution(states, controls));
+        solver.getSolution(states, controls);
+//        std::cerr << "Initial state: " << initialState.toString() << std::endl;
+//        std::cerr << "First state: " << states.front().toString() << std::endl;
+//        std::cerr << "Last state: " << states.back().toString() << std::endl;
+//        std::cerr << "First control: " << controls.front().toString() << std::endl;
+//        std::cerr << "Last control: " << controls.back().toString() << std::endl;
         std::cerr << "Elapsed time: " <<  static_cast<double>(endT - initT) / CLOCKS_PER_SEC * 1000.0 <<" ms."<<std::endl;
     }
 

From 930fefdda29cf4f39c8004d3fff7ff45f4325bdf Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 13 Jun 2018 09:31:14 +0200
Subject: [PATCH 033/194] The variable toBeAdded may have been equal to zero in
 some cycles.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 2200f8591e9..f5294002a7f 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -618,7 +618,6 @@ namespace iDynTree {
                                 newMeshPoint.origin = MeshPointOrigin::FillVariables();
                                 newMeshPoint.type = MeshPointType::State;
                                 while ((mesh->origin != last) && (toBeAdded > 0)){
-                                    toBeAdded = static_cast<unsigned int>(m_totalMeshes - newTotalMeshes);
                                     nextMesh = findNextMeshPoint(mesh); //find next valid mesh
                                     timeDistance = std::abs(nextMesh->time - mesh->time);
 
@@ -632,6 +631,7 @@ namespace iDynTree {
                                             newMeshPoint.time = startTime + m*dT;
                                             addMeshPoint(newMeshPoint);
                                             newTotalMeshes++;
+                                            toBeAdded--;
                                         }
                                         nextMesh = m_meshPoints.begin() + nextPosition;
                                     }

From 2662e8ea78e0c30a0a77583bf62ea170e2c31cde Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 15 Jun 2018 16:20:53 +0200
Subject: [PATCH 034/194] Added some documentation.

---
 .../include/iDynTree/Constraint.h             |   8 +-
 .../include/iDynTree/ConstraintsGroup.h       |   3 +-
 .../iDynTree/ControlledDynamicalSystem.h      |   6 +
 src/optimalcontrol/include/iDynTree/Cost.h    |  65 ++++++++--
 .../include/iDynTree/DynamicalSystem.h        | 103 ++++++++++++++-
 .../include/iDynTree/Integrator.h             | 121 ++++++++++++++++++
 src/optimalcontrol/src/DynamicalSystem.cpp    |   3 +
 7 files changed, 289 insertions(+), 20 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/Constraint.h b/src/optimalcontrol/include/iDynTree/Constraint.h
index 1fcd4ed3914..6bb342116f9 100644
--- a/src/optimalcontrol/include/iDynTree/Constraint.h
+++ b/src/optimalcontrol/include/iDynTree/Constraint.h
@@ -27,10 +27,6 @@ namespace iDynTree {
 
     namespace optimalcontrol {
 
-        /**
-         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
-         * \ingroup iDynTreeExperimental
-         */
 
         /**
          * @brief The Constraint virtual class definition
@@ -38,6 +34,10 @@ namespace iDynTree {
          * Inherit publicly from this class to define a constraint of an optimal control problem.
         */
 
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
         class Constraint {
         public:
 
diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index b6ea0729744..b7bbc3098d7 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -39,8 +39,9 @@ namespace iDynTree {
 
         /**
          * @brief Class grouping constraints associated with a TimeRange.
+         *
          * This class allows to define a set of constraints which are enabled only for a particular time range. This allow to change the constraint structure depending on time. Given a specific time instant, only one constraint is enabled (the one whose TimeRange contains the instant and has the higher initTime).
-         * If the specified time instant does not fall into any time constraint time range, a summy constraint wil be evaluated, i.e. -1 <= 0 <= 1. All the constraints that will be added to the group should have dimension at most equal to maxConstraintSize.
+         * If the specified time instant does not fall into any time constraint time range, a dummy constraint wil be evaluated, i.e. \f$ -1 \leq 0 \leq 1 \f$. All the constraints that will be added to the group should have dimension at most equal to maxConstraintSize.
          * If the constraint size is smaller than the maxConstraintSize, dummy constraints as the above will be added on the bottom. This allow to keep a constant structure even if the constraints have different dimensions.
          * A typical example is when a constraint is enabled only for a certain period of time.
          */
diff --git a/src/optimalcontrol/include/iDynTree/ControlledDynamicalSystem.h b/src/optimalcontrol/include/iDynTree/ControlledDynamicalSystem.h
index 5e972e079bf..8866ed7a0ac 100644
--- a/src/optimalcontrol/include/iDynTree/ControlledDynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/ControlledDynamicalSystem.h
@@ -30,6 +30,12 @@ namespace iDynTree {
          * @brief The ControlledDynamicalSystem class allows to easily connect a DynamicalSystem with a Controller.
          * It defines a controlled dynamical system that can be integrated using an Integrator class.
          */
+
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
         class ControlledDynamicalSystem {
         public:
             ControlledDynamicalSystem();
diff --git a/src/optimalcontrol/include/iDynTree/Cost.h b/src/optimalcontrol/include/iDynTree/Cost.h
index 445d058fdff..8cdb65bc02f 100644
--- a/src/optimalcontrol/include/iDynTree/Cost.h
+++ b/src/optimalcontrol/include/iDynTree/Cost.h
@@ -27,14 +27,15 @@ namespace iDynTree {
     namespace optimalcontrol {
 
         /**
-         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
-         * \ingroup iDynTreeExperimental
+         * @brief The Cost virtual class definition.
+         * Publicly inherit from this class to specify a Cost to be used in a optimal control problem.
          */
 
         /**
-         * @brief The Cost virtual class definition.
-         * Publicly inherit from this class to specify a Cost to be used in a optimal control problem.
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
          */
+
         class Cost {
         public:
 
@@ -66,34 +67,78 @@ namespace iDynTree {
                                         double& costValue) = 0;
 
             /**
-             * @brief costFirstPartialDerivativeWRTState
-             * @param time
-             * @param state
-             * @param control
-             * @param partialDerivative
-             * @return
+             * @brief Evaluate cost first partial derivative wrt the state
+             *
+             * It is the result of \f$\frac{\partial g(t, x, u)}{\partial x}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[out] partialDerivative The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
              */
             virtual bool costFirstPartialDerivativeWRTState(double time,
                                                             const iDynTree::VectorDynSize& state,
                                                             const iDynTree::VectorDynSize& control,
                                                             iDynTree::VectorDynSize& partialDerivative);
 
+            /**
+             * @brief Evaluate cost first partial derivative wrt the control variables
+             *
+             * It is the result of \f$\frac{\partial g(t, x, u)}{\partial u}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[out] partialDerivative The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
             virtual bool costFirstPartialDerivativeWRTControl(double time,
                                                               const iDynTree::VectorDynSize& state,
                                                               const iDynTree::VectorDynSize& control,
                                                               iDynTree::VectorDynSize& partialDerivative);
 
+            /**
+             * @brief Evaluate cost second partial derivative wrt the state variables
+             *
+             * It is the result of \f$\frac{\partial^2 g(t, x, u)}{\partial x^2}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[out] partialDerivative The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
             virtual bool costSecondPartialDerivativeWRTState(double time,
                                                              const iDynTree::VectorDynSize& state,
                                                              const iDynTree::VectorDynSize& control,
                                                              iDynTree::MatrixDynSize& partialDerivative);
 
+
+            /**
+             * @brief Evaluate cost second partial derivative wrt the control
+             *
+             * It is the result of \f$\frac{\partial^2 g(t, x, u)}{\partial u^2}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[out] partialDerivative The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
             virtual bool costSecondPartialDerivativeWRTControl(double time,
                                                                const iDynTree::VectorDynSize& state,
                                                                const iDynTree::VectorDynSize& control,
                                                                iDynTree::MatrixDynSize& partialDerivative);
 
 
+            /**
+             * @brief Evaluate cost second partial derivative wrt the state and control
+             *
+             * It is the result of \f$\frac{\partial^2 g(t, x, u)}{\partial x \partial u}\f$,
+             * thus it has number of rows equals to the number of states and number of cols equal to the number of control inputs.
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[out] partialDerivative The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
             virtual bool costSecondPartialDerivativeWRTStateControl(double time,
                                                                     const iDynTree::VectorDynSize& state,
                                                                     const iDynTree::VectorDynSize& control,
diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index 811169ca65c..862f18b4e4e 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -19,9 +19,6 @@
 
 #include <iDynTree/Core/VectorDynSize.h>
 
-#include <cstddef>
-//#include <memory>
-
 namespace iDynTree {
 
     class MatrixDynSize;
@@ -33,13 +30,25 @@ namespace optimalcontrol {
      * \ingroup iDynTreeExperimental
      */
 
+    /**
+     * @brief DynamicalSystem base class.
+     *
+     * It defines a continuos time dynamical system, i.e. \f$ \dot{x} = f(t,x,u) \f$
+     * Inherit publicly from this class in order to define your custiom dynamical system.
+     */
+
     class DynamicalSystem {
 
     public:
 
         DynamicalSystem() = delete;
 
-        // TODO: add also constant parameters
+        /**
+         * @brief Default constructor
+         *
+         * @param[in] stateSpaceSize Dimension of the state space.
+         * @param[in] controlSpaceSize Dimension of the control space.
+         */
         DynamicalSystem(size_t stateSpaceSize,
                         size_t controlSpaceSize);
 
@@ -47,31 +56,115 @@ namespace optimalcontrol {
 
         virtual ~DynamicalSystem();
 
+        /**
+         * @brief Returns the state space dimension.
+         */
         size_t stateSpaceSize() const;
 
+        /**
+         * @brief Returns the control space dimension.
+         */
         size_t controlSpaceSize() const;
 
+        /**
+         * @brief Computes the system dynamics.
+         *
+         * It return \f$ f(t,x) \f$. Notice that here the dependency from the control input is removed, so that basically we are assuming an autonomous system.
+         * If the system is controlled, the control input will be set separately with the method setControlInput. This was necessary since the Integrator class needs
+         * an autonomous system to be integrated. See ControlledDynamicalSystem class in case you want to join a DynamicalSystem with a Controller.
+         * @param[in] state The state point in which the dynamics is computed.
+         * @param[in] time  The time at which the dynamics is computed.
+         * @param[out] stateDynamics The value of the state derivative.
+         * @return True if successfull.
+         */
         virtual bool dynamics(const VectorDynSize& state,
                               double time,
                               VectorDynSize& stateDynamics) = 0;
 
+        /**
+         * @brief Set the control input to the dynamical system.
+         *
+         * In principle, there is no need to override this method.
+         * This value is stored in an internal buffer which can be accessed through the method controlInput().
+         * @param[in] control The control input value.
+         * @return True if successful, false otherwise (for example if size do not match).
+         */
         virtual bool setControlInput(const VectorDynSize &control);
 
+        /**
+         * @brief Access the control input.
+         *
+         * In principle, there is no need to override this method.
+         * This has to be set with the method setControlInput().
+         * @return Const reference to the control input buffer.
+         */
         virtual const VectorDynSize& controlInput() const;
 
+        /**
+         * @brief Access the control input.
+         *
+         * In principle, there is no need to override this method.
+         * This has to be set with the method setControlInput().
+         * @param[in] index Index at which accessing the control input buffer.
+         * @return Value corresponding to the specified index.
+         */
         virtual double controlInput(unsigned int index) const;
 
+        /**
+         * @brief Access the initial state.
+         *
+         * In principle, there is no need to override this method.
+         * This has to be set with the method setInitialState().
+         * @return Const reference to the initial state buffer.
+         */
         virtual const VectorDynSize& initialState() const;
 
+        /**
+         * @brief Access the initial state.
+         *
+         * In principle, there is no need to override this method.
+         * This has to be set with the method setInitialState().
+         * @param[in] index Index at which accessing the initial state buffer.
+         * @return Value corresponding to the specified index.
+         */
         virtual double initialState(unsigned int index) const;
 
+        /**
+         * @brief Set the initial state to the dynamical system.
+         *
+         * In principle, there is no need to override this method.
+         * This value is stored in an internal buffer which can be accessed through the method initialState().
+         * @param[in] state The initial state value.
+         * @return True if successful, false otherwise (for example if size do not match).
+         */
         virtual bool setInitialState(const VectorDynSize &state);
 
-        //TODO: add also second derivative?
+        /**
+         * @brief Compute the partial derivative of the state dynamics wrt the state.
+         *
+         * Namely it computes, \f$ \frac{\partial f(t,x,u)}{\partial x}\f$.
+         * By default it return false;
+         *
+         * @param[in] state The state value at which computing the partial derivative.
+         * @param[in] control The control value at which computing the partial derivative.
+         * @param[out] dynamicsDerivative The output derivative. It has to be a square matrix with dimension equal to the state size.
+         * @return True if successful, false otherwise (or if not implemented).
+         */
         virtual bool dynamicsStateFirstDerivative(const VectorDynSize& state,
                                                   double time,
                                                   MatrixDynSize& dynamicsDerivative);
 
+        /**
+         * @brief Compute the partial derivative of the state dynamics wrt the control.
+         *
+         * Namely it computes, \f$ \frac{\partial f(t,x,u)}{\partial u}\f$.
+         * By default it return false;
+         *
+         * @param[in] state The state value at which computing the partial derivative.
+         * @param[in] control The control value at which computing the partial derivative.
+         * @param[out] dynamicsDerivative The output derivative. It has to be a matrix with number of rows equal to the state size and number of columns equal to the control size.
+         * @return True if successful, false otherwise (or if not implemented).
+         */
         virtual bool dynamicsControlFirstDerivative(const VectorDynSize& state,
                                                     double time,
                                                     MatrixDynSize& dynamicsDerivative);
diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index 5064ef55d76..5d5cd7dafab 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -36,9 +36,19 @@ namespace optimalcontrol {
              * \ingroup iDynTreeExperimental
              */
 
+            /**
+             * @brief Output of the integrator at a specific time.
+             */
+
             class SolutionElement{
             public:
+                /**
+                 * @brief State value
+                 */
                 VectorDynSize stateAtT;
+                /**
+                 * @brief Time at which stateAtT is computed.
+                 */
                 double time;
             };
 
@@ -55,6 +65,17 @@ namespace optimalcontrol {
                 size_t numberOfStages;
             };
 
+            /**
+             * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+             * \ingroup iDynTreeExperimental
+             */
+
+            /**
+             * @brief Class containing infos about the implemented integrator.
+             *
+             * This class provides a read-only access to the IntegratorInfoData class. An instance of this class is created by the base class Integrator.
+             * All integrators inheriting from this base class should fill in such informations.
+             */
             class IntegratorInfo {
             private:
                 std::shared_ptr<IntegratorInfoData> m_data;
@@ -65,10 +86,22 @@ namespace optimalcontrol {
 
                 IntegratorInfo(const IntegratorInfo &other) = delete;
 
+                /**
+                 * @brief Name of the integration method.
+                 */
                 const std::string &name() const;
 
+                /**
+                 * @brief Tells if the integration method is explicit or not
+                 */
                 bool isExplicit() const;
 
+                /**
+                 * @brief Number of stages of the integration method.
+                 *
+                 * Currently it is not used in th library.
+                 * @return The number of stages of the multistage method.
+                 */
                 size_t numberOfStages() const;
             };
 
@@ -77,32 +110,120 @@ namespace optimalcontrol {
              * \ingroup iDynTreeExperimental
              */
 
+            /**
+             * @brief The Integrator base class.
+             *
+             * Inherit publicly from this class in order to specify a custom integration method.
+             */
             class Integrator {
 
             public:
 
+                /**
+                 * @brief Integrator default constructor.
+                 *
+                 * This constructor allows to create an Integrator object without specifying a DynamicalSystem yet. This has to be done via the method setDynamicalSystem before calling the
+                 * integrate method.
+                 */
                 Integrator();
 
+                /**
+                 * @brief Integrator constructor.
+                 * @param[in] dynamicalSystem Pointer to the DynamicalSystem to be integrated.
+                 */
                 Integrator(const std::shared_ptr<iDynTree::optimalcontrol::DynamicalSystem> dynamicalSystem);
 
+                Integrator(const Integrator &other) = delete;
+
                 virtual ~Integrator();
 
+                /**
+                 * @brief Integrate the dynamical system with specified integration bounds.
+                 *
+                 * This method should be implemented by the custom integrator.
+                 * @param[in] initialTime Lower integration bound.
+                 * @param[in] finalTime Upper integration bound.
+                 * @return true if successfull.
+                 */
                 virtual bool integrate(double initialTime, double finalTime) = 0;
 
+                /**
+                 * @brief Specify the maximum integration step size.
+                 *
+                 * This parameter is considered when integrating a DynamicalSystem.
+                 * @param[in] dT The masimum step size.
+                 * @return true if successfull, false otherwise (for example if dT is not strictly positive)
+                 */
                 bool setMaximumStepSize(const double dT);
 
+                /**
+                 * @brief Return the maximum step size set with setMaximumStepSize.
+                 */
                 double maximumStepSize() const;
 
+                /**
+                 * @brief Set the DynamicalSystem to be considered.
+                 *
+                 * This methods changes the dynamical system only if it was not already set.
+                 * @param[in] dynamicalSystem The dynamical system pointer
+                 * @return true if successfull, false otherwise, for example if a DynamicalSystem was already set.
+                 */
                 bool setDynamicalSystem(const std::shared_ptr<iDynTree::optimalcontrol::DynamicalSystem> dynamicalSystem);
 
+                /**
+                 * @brief Weak pointer to the specified DynamicalSystem.
+                 */
                 const std::weak_ptr<DynamicalSystem> dynamicalSystem() const;
 
+                /**
+                 * @brief Retrieve integration solution at a specified time.
+                 *
+                 * The method integrate should have been called first, and time should be within the integration bounds, otherwise returns false.
+                 *
+                 * It sould not be necessary to override this method.
+                 * @param[in] time Instant of interest.
+                 * @param solution State corresponding to the specified time.
+                 * @return true if successfull.
+                 */
                 virtual bool getSolution(double time, VectorDynSize& solution) const;
 
+                /**
+                 * @brief Retrieve the full buffer of SolutionElement
+                 *
+                 * The method integrate should have been called first, and time should be within the integration bounds, otherwise returns false.
+                 *
+                 * It sould not be necessary to override this method.
+                 * @return A const reference to the internal buffer contaning the full output of the integration routine.
+                 */
                 virtual const std::vector<SolutionElement>& getFullSolution() const;
 
+                /**
+                 * @brief Clear the solution buffer.
+                 */
                 virtual void clearSolution();
 
+
+                /**
+                 * @brief Evaluate the collocation constraint.
+                 *
+                 * In some optimal control solvers, like the MultipleShootingSolver, we need to discretize the dynamical system associated to the optimal control problem. This function evaluates the discretization error according to specified integration method. The Integrator, when integrating a dynamical system, is performing the following discretization:
+                 * \f[
+                 * x_{k+1} = \phi(x_k, x_{k+1}, u_k, u_{k+1}, t)
+                 * \f]
+                 * where \f$ x_{k+1}\f$ is the discretized output. For example, using forward euler, we would have
+                 * \f[
+                 * x_{k+1} = x_k + f(t, x, u)\mathrm{d}T.
+                 * \f]
+                 *
+                 * This function returns the result of \f$ \phi(x_k, x_{k+1}, u_k, u_{k+1}, t) - x_{k+1}\f$.
+                 *
+                 * @param[in] time Instant at which the collocation constraint is evaluated
+                 * @param[in] collocationPoints A vector containing \f$x_k\f$ and \f$x_{k+1}\f$. Notice that \f$x_{k+1}\f$ corresponds to the state at instant (time + dT).
+                 * @param[in] controlInputs A vector containing \f$u_k\f$ and \f$u_{k+1}\f$. Notice that \f$u_{k+1}\f$ is supposed to be applied at instant (time + dT).
+                 * @param[in] dT The time distance between \f$x_k\f$ and \f$x_{k+1}\f$.
+                 * @param[out] constraintValue The result of \f$ \phi(x_k, x_{k+1}, u_k, u_{k+1}, t) - x_{k+1}\f$.
+                 * @return True if successfull, false otherwise (or if not implemented).
+                 */
                 virtual bool evaluateCollocationConstraint(double time, const std::vector<VectorDynSize>& collocationPoints,
                                                            const std::vector<VectorDynSize>& controlInputs, double dT,
                                                            VectorDynSize& constraintValue);
diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp
index 0816198c9da..e03a2356770 100644
--- a/src/optimalcontrol/src/DynamicalSystem.cpp
+++ b/src/optimalcontrol/src/DynamicalSystem.cpp
@@ -18,6 +18,9 @@
 #include <iDynTree/Controller.h>
 #include <iDynTree/Core/Utils.h>
 
+#include <cstddef>
+
+
 namespace iDynTree {
     namespace optimalcontrol {
 

From 968436c22a0e192c453fd9e857ebe2e9f819be4d Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 19 Jun 2018 18:18:48 +0200
Subject: [PATCH 035/194] Considering eventual constraints bias in the OSQP
 interface.

---
 src/optimalcontrol/src/OsqpInterface.cpp | 35 +++++++++++++++---------
 1 file changed, 22 insertions(+), 13 deletions(-)

diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index 9f684250f19..2e503376341 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -242,7 +242,7 @@ namespace iDynTree {
             bool hasBoxConstraints;
             std::shared_ptr<std::vector<size_t>> constraintNNZRows, constraintNNZCols;
             std::shared_ptr<std::vector<size_t>> hessianNNZRows, hessianNNZCols;
-            VectorDynSize variablesBuffer, solutionBuffer;
+            VectorDynSize variablesBuffer, solutionBuffer, constraintsBuffer;
             VectorDynSize costGradient;
             std::shared_ptr<MatrixDynSize> costHessian;
             std::shared_ptr<MatrixDynSize> constraintJacobian;
@@ -507,29 +507,38 @@ namespace iDynTree {
                 return false;
             }
 
+            if (m_pimpl->variablesBuffer.size() != m_pimpl->nv) {
+                m_pimpl->variablesBuffer.resize(m_pimpl->nv);
+                m_pimpl->variablesBuffer.zero();
+            }
+
+            if (!(m_problem->setVariables(m_pimpl->variablesBuffer))) {
+                reportError("OsqpInterface", "solve", "Error while setting to optimization variables to the problem.");
+                return false;
+            }
+
+            m_pimpl->constraintsBuffer.resize(m_pimpl->nc);
+
+            if (!(m_problem->evaluateConstraints(m_pimpl->constraintsBuffer))) {
+                reportError("OsqpInterface", "solve", "Error while evaluating constraints.");
+                return false;
+            }
+
             m_pimpl->unifiedLowerBounds.resize(m_pimpl->nc);
             m_pimpl->unifiedUpperBounds.resize(m_pimpl->nc);
             if (m_pimpl->hasBoxConstraints) {
                 m_pimpl->unifiedLowerBounds.head(m_pimpl->nv) = toEigen(m_pimpl->variablesLowerBounds);
-                m_pimpl->unifiedLowerBounds.tail(m_problem->numberOfConstraints()) = toEigen(m_pimpl->constraintsLowerBounds);
+                m_pimpl->unifiedLowerBounds.tail(m_problem->numberOfConstraints()) =
+                        toEigen(m_pimpl->constraintsLowerBounds) - toEigen(m_pimpl->constraintsBuffer); //remove eventual bias in the constraints
 
                 m_pimpl->unifiedUpperBounds.head(m_pimpl->nv) = toEigen(m_pimpl->variablesUpperBounds);
-                m_pimpl->unifiedUpperBounds.tail(m_problem->numberOfConstraints()) = toEigen(m_pimpl->constraintsUpperBounds);
+                m_pimpl->unifiedUpperBounds.tail(m_problem->numberOfConstraints()) =
+                        toEigen(m_pimpl->constraintsUpperBounds) - toEigen(m_pimpl->constraintsBuffer); //remove eventual bias in the constraints;
             } else {
                 m_pimpl->unifiedLowerBounds = toEigen(m_pimpl->constraintsLowerBounds);
                 m_pimpl->unifiedUpperBounds = toEigen(m_pimpl->constraintsUpperBounds);
             }
 
-            if (m_pimpl->variablesBuffer.size() != m_pimpl->nv) {
-                m_pimpl->variablesBuffer.resize(m_pimpl->nv);
-                m_pimpl->variablesBuffer.zero();
-            }
-
-            if (!(m_problem->setVariables(m_pimpl->variablesBuffer))) {
-                reportError("OsqpInterface", "solve", "Error while setting to optimization variables to the problem.");
-                return false;
-            }
-
             if (!(m_problem->evaluateCostGradient(m_pimpl->costGradient))) {
                 reportError("OsqpInterface", "solve", "Error while retrieving cost gradient.");
                 return false;

From 967ce94ab6fb9df6d7292d6eb5b08144c7555edc Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 20 Aug 2018 11:10:26 +0200
Subject: [PATCH 036/194] Fixed computation of L2Norm cost.

---
 src/optimalcontrol/src/L2NormCost.cpp | 33 +++++++++++++++------------
 1 file changed, 19 insertions(+), 14 deletions(-)

diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index c6d6f5fa8d3..7f5f6b619ad 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -28,7 +28,7 @@ namespace iDynTree {
 
         class TimeVaryingGradient : public TimeVaryingVector {
             MatrixDynSize m_selectorMatrix, m_weightMatrix;
-            MatrixDynSize m_subMatrix; //weightMatrix times selector
+            MatrixDynSize m_hessianMatrix, m_gradientSubMatrix;
             std::shared_ptr<TimeVaryingVector> m_desiredTrajectory;
             VectorDynSize m_outputVector;
         public:
@@ -38,7 +38,10 @@ namespace iDynTree {
             {
                 m_weightMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.rows());
                 toEigen(m_weightMatrix).setIdentity();
-                m_subMatrix = /*m_pimpl->stateWeight * */ m_selectorMatrix;
+                m_hessianMatrix.resize(m_selectorMatrix.cols(), m_selectorMatrix.cols());
+                toEigen(m_hessianMatrix) = toEigen(m_selectorMatrix).transpose() * toEigen(m_selectorMatrix);
+                m_gradientSubMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.cols());
+                toEigen(m_gradientSubMatrix) = -1 * toEigen(m_selectorMatrix);
                 m_outputVector.resize(m_selectorMatrix.cols());
                 m_outputVector.zero();
             }
@@ -67,7 +70,8 @@ namespace iDynTree {
                 }
 
                 m_weightMatrix = weights;
-                toEigen(m_subMatrix) = toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+                toEigen(m_hessianMatrix) = toEigen(m_selectorMatrix).transpose() * toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+                toEigen(m_gradientSubMatrix) = -0.5 * (toEigen(m_weightMatrix).transpose() * toEigen(m_selectorMatrix) + toEigen(m_weightMatrix) * toEigen(m_selectorMatrix));
 
                 return true;
             }
@@ -86,7 +90,7 @@ namespace iDynTree {
                     return m_outputVector;
                 }
 
-                if (desiredPoint.size() != m_subMatrix.rows()) {
+                if (desiredPoint.size() != m_gradientSubMatrix.rows()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The specified desired point at time: " << time << " has size not matching the specified selector.";
                     reportError("TimeVaryingGradient", "getObject", errorMsg.str().c_str());
@@ -95,7 +99,7 @@ namespace iDynTree {
                     return m_outputVector;
                 }
 
-                toEigen(m_outputVector) = -1.0 * toEigen(desiredPoint).transpose() * toEigen(m_subMatrix);
+                toEigen(m_outputVector) = toEigen(desiredPoint).transpose() * toEigen(m_gradientSubMatrix);
 
                 isValid = true;
                 return m_outputVector;
@@ -107,7 +111,8 @@ namespace iDynTree {
                     return false;
                 }
                 m_selectorMatrix = selector;
-                toEigen(m_subMatrix) = toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+                toEigen(m_hessianMatrix) = toEigen(m_selectorMatrix).transpose() * toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
+                toEigen(m_gradientSubMatrix) = -0.5 * (toEigen(m_weightMatrix).transpose() * toEigen(m_selectorMatrix) + toEigen(m_weightMatrix) * toEigen(m_selectorMatrix));
 
                 return true;
             }
@@ -116,8 +121,8 @@ namespace iDynTree {
                 return m_selectorMatrix;
             }
 
-            const MatrixDynSize& subMatrix() {
-                return m_subMatrix;
+            const MatrixDynSize& hessianMatrix() {
+                return m_hessianMatrix;
             }
 
             const MatrixDynSize& weightMatrix() {
@@ -191,7 +196,7 @@ namespace iDynTree {
                     stateGradient = std::make_shared<TimeVaryingGradient>(stateSelector);
                     stateHessian = std::make_shared<TimeInvariantMatrix>();
                     stateHessian->get().resize(stateSelector.cols(), stateSelector.cols());
-                    toEigen(stateHessian->get()) = toEigen(stateGradient->selector()).transpose() * toEigen(stateGradient->subMatrix());
+                    stateHessian->get() = stateGradient->hessianMatrix();
                     stateCostBias = std::make_shared<TimeVaryingBias>(stateGradient);
                 } else {
                     stateGradient = nullptr;
@@ -201,7 +206,7 @@ namespace iDynTree {
                     controlGradient = std::make_shared<TimeVaryingGradient>(controlSelector);
                     controlHessian = std::make_shared<TimeInvariantMatrix>();
                     controlHessian->get().resize(controlSelector.cols(), controlSelector.cols());
-                    toEigen(controlHessian->get()) = toEigen(controlGradient->selector()).transpose() * toEigen(controlGradient->subMatrix());
+                    controlHessian->get() = controlGradient->hessianMatrix();
                     controlCostBias = std::make_shared<TimeVaryingBias>(controlGradient);
                 } else {
                     controlGradient = nullptr;
@@ -310,7 +315,7 @@ namespace iDynTree {
                 return false;
             }
 
-            if (desiredPoint.size() != (m_pimpl->stateGradient->subMatrix().rows())) {
+            if (desiredPoint.size() != (m_pimpl->stateGradient->selector().rows())) {
                 reportError("L2NormCost", "setStateDesiredPoint", "The desiredPoint size do not match the dimension of the specified selector.");
                 return false;
             }
@@ -361,7 +366,7 @@ namespace iDynTree {
                 return false;
             }
 
-            if (desiredPoint.size() != (m_pimpl->controlGradient->subMatrix().rows())) {
+            if (desiredPoint.size() != (m_pimpl->controlGradient->selector().rows())) {
                 reportError("L2NormCost", "setControlDesiredPoint", "The desiredPoint size do not match the dimension of the specified selector.");
                 return false;
             }
@@ -391,7 +396,7 @@ namespace iDynTree {
                 reportError("L2NormCost", "updatStateSelector", "Error when updating state selector.");
                 return false;
             }
-            toEigen(m_pimpl->stateHessian->get()) = toEigen(m_pimpl->stateGradient->selector()).transpose() * toEigen(m_pimpl->stateGradient->subMatrix());
+            m_pimpl->stateHessian->get() = m_pimpl->stateGradient->hessianMatrix();
             return true;
         }
 
@@ -401,7 +406,7 @@ namespace iDynTree {
                 reportError("L2NormCost", "updatControlSelector", "Error when updating state selector.");
                 return false;
             }
-            toEigen(m_pimpl->controlHessian->get()) = toEigen(m_pimpl->controlGradient->selector()).transpose() * toEigen(m_pimpl->controlGradient->subMatrix());
+            m_pimpl->controlHessian->get() = m_pimpl->controlGradient->hessianMatrix();
             return true;
         }
 

From 3a2b972203750d0b94ba15751151220df107aca4 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 20 Aug 2018 12:03:09 +0200
Subject: [PATCH 037/194] Fixes after codacy and @traversaro reviews.

---
 cmake/iDynTreeDependencies.cmake                  |  1 +
 src/optimalcontrol/CMakeLists.txt                 | 11 +----------
 src/optimalcontrol/src/MultipleShootingSolver.cpp |  2 +-
 src/optimalcontrol/tests/CMakeLists.txt           |  2 +-
 4 files changed, 4 insertions(+), 12 deletions(-)

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 79e1c196960..0529bbb9d8d 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -54,4 +54,5 @@ endif()
 idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
+idyntree_handle_dependency(OsqpEigen)
 
diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 417d0f245a8..039a8187e7f 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -72,22 +72,13 @@ if (IDYNTREE_USES_IPOPT)
     list(APPEND INCLUDE_LIST ${IPOPT_INCLUDE_DIRS})
 endif()
 
-find_package(OsqpEigen QUIET)
-if (${OsqpEigen_FOUND})
-    option(OC_USE_OSQP "Use OSQP as a solver in optimalcontrol." ON)
-else()
-    set(OC_USE_OSQP OFF)
-endif()
-
-if (OC_USE_OSQP)
+if (IDYNTREE_USES_OSQPEIGEN)
     list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/OsqpInterface.h)
     list(APPEND OPTIMIZERS_SOURCES src/OsqpInterface.cpp)
     list(APPEND LINK_LIST   OsqpEigen::OsqpEigen osqp::osqp)
 endif()
 
 
-
-
 add_library(${libraryname} ${PUBLIC_HEADERS} ${INTEGRATORS_PUBLIC_HEADERS} ${OCSOLVERS_PUBLIC_HEADERS} ${OPTIMIZERS_HEADERS} ${SOURCES} ${OPTIMIZERS_SOURCES})
 target_include_directories(${libraryname} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
 target_include_directories(${libraryname} SYSTEM PRIVATE ${INCLUDE_LIST})
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index f5294002a7f..f4ca5b5f822 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -326,7 +326,7 @@ namespace iDynTree {
                 if ((lastControlMeshIterator->origin == MeshPointOrigin::LastPoint()) && (m_integrator->info().isExplicit())) {//the last control input would have no effect
                     controlMeshes--;
                     setIgnoredMesh(lastControlMeshIterator);
-                    lastControlMeshIterator--;
+                    --lastControlMeshIterator;
                 }
 
                 newMeshPoint.origin = MeshPointOrigin::LastPoint();
diff --git a/src/optimalcontrol/tests/CMakeLists.txt b/src/optimalcontrol/tests/CMakeLists.txt
index 428ea93866e..3b6afc0aa9a 100644
--- a/src/optimalcontrol/tests/CMakeLists.txt
+++ b/src/optimalcontrol/tests/CMakeLists.txt
@@ -22,7 +22,7 @@ if (IDYNTREE_USES_IPOPT)
     add_oc_test(OptimalControlIpopt)
 endif()
 
-if (OC_USE_OSQP)
+if (IDYNTREE_USES_OSQPEIGEN)
     add_oc_test(LinearOCOsqp)
 endif()
 

From 2ff9dd061b5999da02a04f44d6205f3f50ac5b6a Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 20 Aug 2018 18:50:19 +0200
Subject: [PATCH 038/194] Added missing licences.

---
 src/optimalcontrol/CMakeLists.txt       | 8 ++++++++
 src/optimalcontrol/tests/CMakeLists.txt | 8 ++++++++
 2 files changed, 16 insertions(+)

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 039a8187e7f..4d153353658 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -1,3 +1,11 @@
+# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+#
+# Licensed under either the GNU Lesser General Public License v3.0 :
+# https://www.gnu.org/licenses/lgpl-3.0.html
+# or the GNU Lesser General Public License v2.1 :
+# https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+# at your option.
+
 cmake_minimum_required(VERSION 3.0)
 
 project(idyntree_optimalcontrol)
diff --git a/src/optimalcontrol/tests/CMakeLists.txt b/src/optimalcontrol/tests/CMakeLists.txt
index 3b6afc0aa9a..5c683f65de1 100644
--- a/src/optimalcontrol/tests/CMakeLists.txt
+++ b/src/optimalcontrol/tests/CMakeLists.txt
@@ -1,3 +1,11 @@
+# Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+#
+# Licensed under either the GNU Lesser General Public License v3.0 :
+# https://www.gnu.org/licenses/lgpl-3.0.html
+# or the GNU Lesser General Public License v2.1 :
+# https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+# at your option.
+
 get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_oc_test classname)
     set(testsrc ${classname}Test.cpp)

From b768c11428946d4d556898ec5420d96c5a1a4350 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 29 Aug 2018 17:21:58 +0200
Subject: [PATCH 039/194] Added few asserts in Eigenhelpers test.

---
 src/core/tests/EigenHelpersUnitTest.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/core/tests/EigenHelpersUnitTest.cpp b/src/core/tests/EigenHelpersUnitTest.cpp
index 70377c9c7c3..cfae4be80c2 100644
--- a/src/core/tests/EigenHelpersUnitTest.cpp
+++ b/src/core/tests/EigenHelpersUnitTest.cpp
@@ -34,14 +34,15 @@ void testSpatialVectors()
 }
 
 void testSpanToEigen(const Vector3& input) {
-    //Checks if compiles
     Vector3 randVec, check;
     getRandomVector(randVec);
     Span<double> spanCheck = make_span(check);
     toEigen(spanCheck) = toEigen(randVec);
+    ASSERT_EQUAL_VECTOR(randVec, check);
     toEigen(make_span(check)) = toEigen(randVec);
     toEigen(randVec) = toEigen(make_span(check));
     toEigen(check) = toEigen(make_span(input));
+    ASSERT_EQUAL_VECTOR(input, check);
 }
 
 int main()

From 2f621bf89dc24f58e197b7e308c670b2d855fe63 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 29 Aug 2018 17:23:07 +0200
Subject: [PATCH 040/194] Modified L2Norm to consider IndexRanges.

The cost evaluation is done by applying the L2Norm definition rather than resorting to the corresponding quadratic form. The idea is that, by selecting directly the variables of interestest from the total state vector, we reduce the number of floating point operations.
---
 .../include/iDynTree/L2NormCost.h             |  14 +-
 .../include/iDynTree/QuadraticLikeCost.h      |   2 +-
 src/optimalcontrol/src/L2NormCost.cpp         | 289 +++++++++++++-----
 src/optimalcontrol/tests/CMakeLists.txt       |   1 +
 src/optimalcontrol/tests/L2NormTest.cpp       |  75 +++++
 5 files changed, 297 insertions(+), 84 deletions(-)
 create mode 100644 src/optimalcontrol/tests/L2NormTest.cpp

diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index 65851e5249f..bcb753dfd3c 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -20,6 +20,7 @@
 
 #include <iDynTree/QuadraticLikeCost.h>
 #include <iDynTree/TimeVaryingObject.h>
+#include <iDynTree/Core/Utils.h>
 #include <memory>
 #include <string>
 
@@ -41,16 +42,17 @@ namespace iDynTree {
 
         public:
 
-            L2NormCost(const std::string& name, unsigned int stateDimension, unsigned int controlDimension); //assume identity as selector. Set the dimension to 0 to avoid weighting either the state or the cost
+            L2NormCost(const std::string& name, unsigned int stateDimension, unsigned int controlDimension); //assume identity as selector. Set the dimension to 0 to avoid weighting either the state or the control
 
-            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector, const MatrixDynSize& controlSelector); //The selector premultiplies the state and the control in the L2-norm. Set some dimension to 0 to avoid weighting either the state or the cost
+            L2NormCost(const std::string& name, const MatrixDynSize& stateSelector, const MatrixDynSize& controlSelector); //The selector premultiplies the state and the control in the L2-norm. Set some dimension to 0 to avoid weighting either the state or the control
+
+            L2NormCost(const std::string& name, const IndexRange& stateSelector, unsigned int totalStateDimension,
+                       const IndexRange& controlSelector, unsigned int totalControlDimension); //Pass an invalid range to avoid weighting either the state or the control.
 
             L2NormCost(const L2NormCost& other) = delete;
 
             ~L2NormCost();
 
-            void computeConstantPart(bool addItToTheCost = true); //by default the constant part is not added since it adds computational burden. Use it if you need the cost evaluation to be exactly equal to an L2 norm
-
             bool setStateWeight(const MatrixDynSize& stateWeights);
 
             bool setStateDesiredPoint(const VectorDynSize& desiredPoint);
@@ -67,6 +69,10 @@ namespace iDynTree {
 
             bool updatControlSelector(const MatrixDynSize& controlSelector);
 
+            virtual bool costEvaluation(double time,
+                                        const iDynTree::VectorDynSize& state,
+                                        const iDynTree::VectorDynSize& control,
+                                        double& costValue) final;
         };
 
     }
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
index 05e6c5bbfbd..27eaf08167f 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
@@ -39,7 +39,7 @@ namespace iDynTree {
            virtual bool costEvaluation(double time,
                                        const iDynTree::VectorDynSize& state,
                                        const iDynTree::VectorDynSize& control,
-                                       double& costValue) final;//0.5 xT H x + gx + c
+                                       double& costValue) override;//0.5 xT H x + gx + c
 
            virtual bool costFirstPartialDerivativeWRTState(double time,
                                                            const iDynTree::VectorDynSize& state,
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 7f5f6b619ad..1ed51f1885a 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -18,14 +18,72 @@
 #include <iDynTree/Core/MatrixDynSize.h>
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/Span.h>
 #include <cassert>
-
 #include <Eigen/Dense>
 #include <iDynTree/Core/EigenHelpers.h>
 
 namespace iDynTree {
     namespace optimalcontrol {
 
+    //
+    // Implementation of selector matrix
+    //
+        class Selector {
+        protected:
+            VectorDynSize m_selected;
+        public:
+            Selector(unsigned int selectedSize) : m_selected(selectedSize) { }
+
+            virtual ~Selector();
+
+            virtual const VectorDynSize& select(const VectorDynSize& fullVector) = 0;
+
+            unsigned int size() const {
+                return m_selected.size();
+            }
+
+        };
+        Selector::~Selector(){}
+
+        class IndexSelector : public Selector {
+            IndexRange m_selectedIndex;
+        public:
+            IndexSelector(const IndexRange& selectedRange)
+                : Selector(static_cast<unsigned int>(selectedRange.size))
+                , m_selectedIndex(selectedRange) {}
+
+            ~IndexSelector() override;
+
+            virtual const VectorDynSize& select(const VectorDynSize& fullVector) final
+            {
+                m_selected = make_span(fullVector).subspan(m_selectedIndex.offset, m_selectedIndex.size);
+                return m_selected;
+            }
+        };
+        IndexSelector::~IndexSelector(){}
+
+        class MatrixSelector : public Selector {
+            MatrixDynSize m_selectorMatrix;
+        public:
+            MatrixSelector(const MatrixDynSize& selectorMatrix)
+                : Selector(selectorMatrix.rows())
+                , m_selectorMatrix(selectorMatrix) {}
+
+            ~MatrixSelector() override;
+
+            virtual const VectorDynSize& select(const VectorDynSize& fullVector) final
+            {
+                iDynTree::toEigen(m_selected) = iDynTree::toEigen(m_selectorMatrix) * iDynTree::toEigen(fullVector);
+                return m_selected;
+            }
+        };
+        MatrixSelector::~MatrixSelector(){}
+
+    //
+    // Implementation of TimeVaryingGradient
+    //
+
         class TimeVaryingGradient : public TimeVaryingVector {
             MatrixDynSize m_selectorMatrix, m_weightMatrix;
             MatrixDynSize m_hessianMatrix, m_gradientSubMatrix;
@@ -135,69 +193,28 @@ namespace iDynTree {
         };
         TimeVaryingGradient::~TimeVaryingGradient() {};
 
-        class TimeVaryingBias : public TimeVaryingDouble {
-            std::shared_ptr<TimeVaryingGradient> m_associatedGradient;
-            double m_output;
-        public:
-            TimeVaryingBias(std::shared_ptr<TimeVaryingGradient> associatedGradient)
-            :m_associatedGradient(associatedGradient)
-            { }
-
-            ~TimeVaryingBias() override;
-
-            virtual const double& get(double time, bool &isValid) override{
-                if (!(m_associatedGradient)) {
-                    isValid = false;
-                    m_output = 0.0;
-                    return m_output;
-                }
-
-                if (!(m_associatedGradient->desiredTrajectory())) {
-                    isValid = true;
-                    m_output = 0.0;
-                    return m_output;
-                }
-
-                bool ok = false;
-                const VectorDynSize &desiredPoint = m_associatedGradient->desiredTrajectory()->get(time, ok);
-
-                if (!ok) {
-                    isValid = false;
-                    m_output = 0.0;
-                    return m_output;
-                }
-
-                if (desiredPoint.size() != m_associatedGradient->weightMatrix().rows()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The specified desired point at time: " << time << " has size not matching the specified weight matrix.";
-                    reportError("TimeVaryingBias", "getObject", errorMsg.str().c_str());
-                    isValid = false;
-                    m_output = 0.0;
-                    return m_output;
-                }
-
-                isValid = true;
-                m_output = 0.5 * toEigen(desiredPoint).transpose() * toEigen(m_associatedGradient->weightMatrix()) * toEigen(desiredPoint);
-                return m_output;
-            }
-        };
-        TimeVaryingBias::~TimeVaryingBias() { }
-
+        //
+        // End of implementation of TimeVaryingGradient
+        //
 
+        //
+        // Definition of pimpl
+        //
         class L2NormCost::L2NormCostImplementation {
             public:
             std::shared_ptr<TimeVaryingGradient> stateGradient = nullptr, controlGradient = nullptr;
             std::shared_ptr<TimeInvariantMatrix> stateHessian, controlHessian;
-            std::shared_ptr<TimeVaryingBias> stateCostBias, controlCostBias;
-            bool addConstantPart = false;
+            std::shared_ptr<Selector> stateSelector_ptr, controlSelector_ptr;
+            iDynTree::VectorDynSize stateCostBuffer, controlCostBuffer;
 
-            void initialize(const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector) {
+            void initializeHessianAndGradient(const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector) {
                 if ((stateSelector.rows() != 0) && (stateSelector.cols() != 0)) {
                     stateGradient = std::make_shared<TimeVaryingGradient>(stateSelector);
                     stateHessian = std::make_shared<TimeInvariantMatrix>();
                     stateHessian->get().resize(stateSelector.cols(), stateSelector.cols());
                     stateHessian->get() = stateGradient->hessianMatrix();
-                    stateCostBias = std::make_shared<TimeVaryingBias>(stateGradient);
+                    stateCostBuffer.resize(stateSelector.rows());
+                    stateCostBuffer.zero();
                 } else {
                     stateGradient = nullptr;
                 }
@@ -207,21 +224,49 @@ namespace iDynTree {
                     controlHessian = std::make_shared<TimeInvariantMatrix>();
                     controlHessian->get().resize(controlSelector.cols(), controlSelector.cols());
                     controlHessian->get() = controlGradient->hessianMatrix();
-                    controlCostBias = std::make_shared<TimeVaryingBias>(controlGradient);
+                    controlCostBuffer.resize(controlSelector.rows());
+                    controlCostBuffer.zero();
                 } else {
                     controlGradient = nullptr;
                 }
             }
 
-            void initialize(unsigned int stateDimension, unsigned int controlDimension) {
+            void initializeHessianAndGradient(unsigned int stateDimension, unsigned int controlDimension) {
                 MatrixDynSize stateSelector(stateDimension, stateDimension), controlSelector(controlDimension, controlDimension);
                 toEigen(stateSelector).setIdentity();
                 toEigen(controlSelector).setIdentity();
-                initialize(stateSelector, controlSelector);
+                initializeHessianAndGradient(stateSelector, controlSelector);
+            }
+
+            void initializeHessianAndGradient(const IndexRange &stateSelector, unsigned int totalStateDimension, const IndexRange &controlSelector, unsigned int totalControlDimension) {
+                MatrixDynSize stateSelectorMatrix, controlSelectorMatrix;
+
+                if (stateSelector.isValid()) {
+                    stateSelectorMatrix.resize(static_cast<unsigned int>(stateSelector.size), totalStateDimension);
+                    iDynTree::toEigen(stateSelectorMatrix).block(0, stateSelector.offset, stateSelector.size, stateSelector.size).setIdentity();
+                } else {
+                    stateSelectorMatrix.resize(0,0);
+                }
+
+                if (controlSelector.isValid()) {
+                    controlSelectorMatrix.resize(static_cast<unsigned int>(controlSelector.size), totalControlDimension);
+                    iDynTree::toEigen(controlSelectorMatrix).block(0, controlSelector.offset, controlSelector.size, controlSelector.size).setIdentity();
+                } else {
+                    controlSelectorMatrix.resize(0,0);
+                }
+
+                initializeHessianAndGradient(stateSelectorMatrix, controlSelectorMatrix);
             }
 
         };
 
+        //
+        //End of definition of pimpl
+        //
+
+        //
+        //Implementation of L2NormCost
+        //
 
         L2NormCost::L2NormCost(const std::string &name, unsigned int stateDimension, unsigned int controlDimension)
         : QuadraticLikeCost(name)
@@ -229,16 +274,24 @@ namespace iDynTree {
         {
             assert(m_pimpl);
 
-            m_pimpl->initialize(stateDimension, controlDimension);
+            m_pimpl->initializeHessianAndGradient(stateDimension, controlDimension);
 
             if (m_pimpl->stateGradient) {
                 m_timeVaryingStateHessian = m_pimpl->stateHessian;
                 m_timeVaryingStateGradient = m_pimpl->stateGradient;
+                IndexRange stateRange;
+                stateRange.offset = 0;
+                stateRange.size = stateDimension;
+                m_pimpl->stateSelector_ptr = std::make_shared<IndexSelector>(stateRange);
             }
 
             if (m_pimpl->controlGradient) {
                 m_timeVaryingControlHessian = m_pimpl->controlHessian;
                 m_timeVaryingControlGradient = m_pimpl->controlGradient;
+                IndexRange controlRange;
+                controlRange.offset = 0;
+                controlRange.size = stateDimension;
+                m_pimpl->controlSelector_ptr = std::make_shared<IndexSelector>(controlRange);
             }
         }
 
@@ -246,45 +299,45 @@ namespace iDynTree {
         : QuadraticLikeCost(name)
         , m_pimpl(new L2NormCostImplementation)
         {
-            m_pimpl->initialize(stateSelector, controlSelector);
+            m_pimpl->initializeHessianAndGradient(stateSelector, controlSelector);
 
             if (m_pimpl->stateGradient) {
                 m_timeVaryingStateHessian = m_pimpl->stateHessian;
                 m_timeVaryingStateGradient = m_pimpl->stateGradient;
+                m_pimpl->stateSelector_ptr = std::make_shared<MatrixSelector>(stateSelector);
             }
 
             if (m_pimpl->controlGradient) {
                 m_timeVaryingControlHessian = m_pimpl->controlHessian;
                 m_timeVaryingControlGradient = m_pimpl->controlGradient;
+                m_pimpl->controlSelector_ptr = std::make_shared<MatrixSelector>(controlSelector);
             }
         }
 
-        L2NormCost::~L2NormCost()
+        L2NormCost::L2NormCost(const std::string &name, const IndexRange &stateSelector, unsigned int totalStateDimension, const IndexRange &controlSelector, unsigned int totalControlDimension)
+            : QuadraticLikeCost (name)
+            , m_pimpl(new L2NormCostImplementation)
         {
-            if (m_pimpl){
-                delete m_pimpl;
-                m_pimpl = nullptr;
+            m_pimpl->initializeHessianAndGradient(stateSelector, totalStateDimension, controlSelector, totalControlDimension);
+
+            if (m_pimpl->stateGradient) {
+                m_timeVaryingStateHessian = m_pimpl->stateHessian;
+                m_timeVaryingStateGradient = m_pimpl->stateGradient;
+                m_pimpl->stateSelector_ptr = std::make_shared<IndexSelector>(stateSelector);
+            }
+
+            if (m_pimpl->controlGradient) {
+                m_timeVaryingControlHessian = m_pimpl->controlHessian;
+                m_timeVaryingControlGradient = m_pimpl->controlGradient;
+                m_pimpl->controlSelector_ptr = std::make_shared<IndexSelector>(controlSelector);
             }
         }
 
-        void L2NormCost::computeConstantPart(bool addItToTheCost)
+        L2NormCost::~L2NormCost()
         {
-            m_pimpl->addConstantPart = addItToTheCost;
-            if (m_pimpl->addConstantPart) {
-                if (m_pimpl->stateGradient) {
-                    m_timeVaryingStateCostBias = m_pimpl->stateCostBias;
-                }
-                if (m_pimpl->controlGradient) {
-                    m_timeVaryingControlCostBias = m_pimpl->controlCostBias;
-                }
-            } else {
-                std::shared_ptr<TimeVaryingDouble> stateBias(new TimeInvariantDouble(0.0)), controlBias(new TimeInvariantDouble(0.0));
-                if (m_pimpl->stateGradient) {
-                    m_timeVaryingStateCostBias = stateBias;
-                }
-                if (m_pimpl->controlGradient) {
-                    m_timeVaryingControlCostBias = controlBias;
-                }
+            if (m_pimpl){
+                delete m_pimpl;
+                m_pimpl = nullptr;
             }
         }
 
@@ -410,6 +463,84 @@ namespace iDynTree {
             return true;
         }
 
+        bool L2NormCost::costEvaluation(double time, const VectorDynSize &state, const VectorDynSize &control, double &costValue)
+        {
+            double stateCost = 0, controlCost = 0;
+
+            bool isValid = false;
+            if (m_pimpl->stateGradient) {
+
+                if (state.size() != m_pimpl->stateGradient->selector().cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state dimension is not the one expected from the specified selector.";
+                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                iDynTree::toEigen(m_pimpl->stateCostBuffer) = iDynTree::toEigen(m_pimpl->stateSelector_ptr->select(state));
+
+                if (m_pimpl->stateGradient->desiredTrajectory()) {
+                    const VectorDynSize& desired = m_pimpl->stateGradient->desiredTrajectory()->get(time, isValid);
+
+                    if (!isValid) {
+                        std::ostringstream errorMsg;
+                        errorMsg << "Unable to retrieve a valid desired state at time: " << time << ".";
+                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                        return false;
+                    }
+
+                    if (m_pimpl->stateSelector_ptr->size() != desired.size()) {
+                        std::ostringstream errorMsg;
+                        errorMsg << "The desired state dimension does not match the size of the selector at time: " << time << ".";
+                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                        return false;
+                    }
+
+                    iDynTree::toEigen(m_pimpl->stateCostBuffer) -= iDynTree::toEigen(desired);
+                }
+
+                stateCost = 0.5 * iDynTree::toEigen(m_pimpl->stateCostBuffer).transpose() * (iDynTree::toEigen(m_pimpl->stateGradient->weightMatrix()) * iDynTree::toEigen(m_pimpl->stateCostBuffer));
+            }
+
+            if (m_pimpl->controlGradient) {
+
+                if (control.size() != m_pimpl->controlGradient->selector().cols()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control dimension is not the one expected from the specified selector.";
+                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
+
+                iDynTree::toEigen(m_pimpl->controlCostBuffer) = iDynTree::toEigen(m_pimpl->controlSelector_ptr->select(control));
+
+                if (m_pimpl->controlGradient->desiredTrajectory()) {
+                    const VectorDynSize& desired = m_pimpl->controlGradient->desiredTrajectory()->get(time, isValid);
+
+                    if (!isValid) {
+                        std::ostringstream errorMsg;
+                        errorMsg << "Unable to retrieve a valid desired control at time: " << time << ".";
+                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                        return false;
+                    }
+
+                    if (m_pimpl->controlSelector_ptr->size() != desired.size()) {
+                        std::ostringstream errorMsg;
+                        errorMsg << "The desired control dimension does not match the size of the selector at time: " << time << ".";
+                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                        return false;
+                    }
+
+                    iDynTree::toEigen(m_pimpl->controlCostBuffer) -= iDynTree::toEigen(desired);
+                }
+
+                controlCost = 0.5 * iDynTree::toEigen(m_pimpl->controlCostBuffer).transpose() * (iDynTree::toEigen(m_pimpl->controlGradient->weightMatrix()) * iDynTree::toEigen(m_pimpl->controlCostBuffer));
+            }
+
+            costValue = stateCost + controlCost;
+
+            return true;
+        }
+
     }
 }
 
diff --git a/src/optimalcontrol/tests/CMakeLists.txt b/src/optimalcontrol/tests/CMakeLists.txt
index 5c683f65de1..32bb79f3ffa 100644
--- a/src/optimalcontrol/tests/CMakeLists.txt
+++ b/src/optimalcontrol/tests/CMakeLists.txt
@@ -25,6 +25,7 @@ add_oc_test(ConstraintsGroup)
 add_oc_test(Integrators)
 add_oc_test(OCProblem)
 add_oc_test(MultipleShooting)
+add_oc_test(L2Norm)
 if (IDYNTREE_USES_IPOPT)
     add_oc_test(Optimizer)
     add_oc_test(OptimalControlIpopt)
diff --git a/src/optimalcontrol/tests/L2NormTest.cpp b/src/optimalcontrol/tests/L2NormTest.cpp
new file mode 100644
index 00000000000..d232b3882ba
--- /dev/null
+++ b/src/optimalcontrol/tests/L2NormTest.cpp
@@ -0,0 +1,75 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Stefano Dafarra
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#include <iDynTree/L2NormCost.h>
+#include <iDynTree/TimeVaryingObject.h>
+#include <iDynTree/Core/TestUtils.h>
+#include <iDynTree/Core/EigenHelpers.h>
+#include <memory>
+
+int main() {
+
+    iDynTree::VectorDynSize state(5), desiredStateValue(2), control(10);
+    iDynTree::IndexRange stateRange, controlRange;
+
+    iDynTree::getRandomVector(state);
+    iDynTree::getRandomVector(desiredStateValue);
+    iDynTree::getRandomVector(control);
+
+    stateRange.offset = 1;
+    stateRange.size = 2;
+
+    controlRange.offset = 3;
+    controlRange.size = 4;
+
+    std::shared_ptr<iDynTree::optimalcontrol::L2NormCost> stateCost = std::make_shared<iDynTree::optimalcontrol::L2NormCost>("stateCost", stateRange, 5, iDynTree::IndexRange::InvalidRange(), 1);
+    std::shared_ptr<iDynTree::optimalcontrol::Cost> controlCost = std::make_shared<iDynTree::optimalcontrol::L2NormCost>("controlCost", iDynTree::IndexRange::InvalidRange(), 1, controlRange, 10);
+    std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> desiredState = std::make_shared<iDynTree::optimalcontrol::TimeInvariantVector>(desiredStateValue);
+
+
+    iDynTree::MatrixDynSize stateWeight(2,2);
+    stateWeight(0,0) = 2;
+    stateWeight(1,1) = 2;
+
+    double controlCostCHeck = 0.5 * iDynTree::toEigen(control).segment(controlRange.offset, controlRange.size).squaredNorm();
+    double controlCostOutput;
+
+    bool ok = controlCost ->costEvaluation(0.0, state, control, controlCostOutput);
+
+    ASSERT_IS_TRUE(ok);
+    ASSERT_EQUAL_DOUBLE(controlCostCHeck, controlCostOutput);
+
+    ok = stateCost->setStateWeight(stateWeight);
+    ASSERT_IS_TRUE(ok);
+
+    ok = stateCost->setStateDesiredTrajectory(desiredState);
+    ASSERT_IS_TRUE(ok);
+
+    double stateCostCheck = (iDynTree::toEigen(state).segment(stateRange.offset, stateRange.size) - iDynTree::toEigen(desiredStateValue)).squaredNorm();
+    double stateCostOutput;
+
+    ok = stateCost->costEvaluation(0.0, state, control, stateCostOutput);
+
+    ASSERT_IS_TRUE(ok);
+    ASSERT_EQUAL_DOUBLE(stateCostCheck, stateCostOutput);
+
+    iDynTree::VectorDynSize dummyVector;
+    iDynTree::MatrixDynSize dummyMatrix;
+
+    ok = stateCost->costFirstPartialDerivativeWRTState(0.0, state, control, dummyVector);
+    ASSERT_IS_TRUE(ok);
+    ok = stateCost->costFirstPartialDerivativeWRTControl(0.0, state, control, dummyVector);
+    ASSERT_IS_TRUE(ok);
+    ok = stateCost->costSecondPartialDerivativeWRTState(0.0, state, control, dummyMatrix);
+    ASSERT_IS_TRUE(ok);
+    ok = stateCost->costSecondPartialDerivativeWRTControl(0.0, state, control, dummyMatrix);
+    ASSERT_IS_TRUE(ok);
+
+
+    return EXIT_SUCCESS;
+
+}

From 3299042056b3c400db3b1f31036f7389bec74666 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 30 Aug 2018 16:22:41 +0200
Subject: [PATCH 041/194] Complete refactor of L2NormCost. It does not inherit
 from quadratic cost anymore.

---
 .../include/iDynTree/L2NormCost.h             |  26 +-
 src/optimalcontrol/src/L2NormCost.cpp         | 465 ++++++++----------
 src/optimalcontrol/src/QuadraticLikeCost.cpp  |   8 +-
 3 files changed, 245 insertions(+), 254 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index bcb753dfd3c..38017534b9a 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -18,7 +18,7 @@
 #ifndef IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
 #define IDYNTREE_OPTIMALCONTROL_L2NORMCOST_H
 
-#include <iDynTree/QuadraticLikeCost.h>
+#include <iDynTree/Cost.h>
 #include <iDynTree/TimeVaryingObject.h>
 #include <iDynTree/Core/Utils.h>
 #include <memory>
@@ -36,7 +36,7 @@ namespace iDynTree {
          * \ingroup iDynTreeExperimental
          */
 
-        class L2NormCost : public QuadraticLikeCost {
+        class L2NormCost : public Cost {
             class L2NormCostImplementation;
             L2NormCostImplementation *m_pimpl;
 
@@ -73,6 +73,28 @@ namespace iDynTree {
                                         const iDynTree::VectorDynSize& state,
                                         const iDynTree::VectorDynSize& control,
                                         double& costValue) final;
+
+            virtual bool costFirstPartialDerivativeWRTState(double time,
+                                                            const iDynTree::VectorDynSize& state, const iDynTree::VectorDynSize&,
+                                                            iDynTree::VectorDynSize& partialDerivative) final;
+
+            virtual bool costFirstPartialDerivativeWRTControl(double time, const iDynTree::VectorDynSize&,
+                                                              const iDynTree::VectorDynSize& control,
+                                                              iDynTree::VectorDynSize& partialDerivative) final;
+
+            virtual bool costSecondPartialDerivativeWRTState(double,
+                                                             const iDynTree::VectorDynSize& state, const iDynTree::VectorDynSize&,
+                                                             iDynTree::MatrixDynSize& partialDerivative) final;
+
+            virtual bool costSecondPartialDerivativeWRTControl(double, const iDynTree::VectorDynSize&,
+                                                               const iDynTree::VectorDynSize& control,
+                                                               iDynTree::MatrixDynSize& partialDerivative) final;
+
+
+            virtual bool costSecondPartialDerivativeWRTStateControl(double,
+                                                                    const iDynTree::VectorDynSize& state,
+                                                                    const iDynTree::VectorDynSize& control,
+                                                                    iDynTree::MatrixDynSize& partialDerivative) final;
         };
 
     }
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 1ed51f1885a..7409a932e7a 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -32,6 +32,7 @@ namespace iDynTree {
         class Selector {
         protected:
             VectorDynSize m_selected;
+            MatrixDynSize m_selectionMatrix;
         public:
             Selector(unsigned int selectedSize) : m_selected(selectedSize) { }
 
@@ -39,6 +40,10 @@ namespace iDynTree {
 
             virtual const VectorDynSize& select(const VectorDynSize& fullVector) = 0;
 
+            virtual const MatrixDynSize& asSelectorMatrix() {
+                return m_selectionMatrix;
+            }
+
             unsigned int size() const {
                 return m_selected.size();
             }
@@ -49,9 +54,13 @@ namespace iDynTree {
         class IndexSelector : public Selector {
             IndexRange m_selectedIndex;
         public:
-            IndexSelector(const IndexRange& selectedRange)
+            IndexSelector(const IndexRange& selectedRange, unsigned int totalSize)
                 : Selector(static_cast<unsigned int>(selectedRange.size))
-                , m_selectedIndex(selectedRange) {}
+                , m_selectedIndex(selectedRange)
+            {
+                m_selectionMatrix.resize(static_cast<unsigned int>(selectedRange.size), totalSize);
+                toEigen(m_selectionMatrix).block(0, selectedRange.offset, selectedRange.size, selectedRange.size).setIdentity();
+            }
 
             ~IndexSelector() override;
 
@@ -64,202 +73,216 @@ namespace iDynTree {
         IndexSelector::~IndexSelector(){}
 
         class MatrixSelector : public Selector {
-            MatrixDynSize m_selectorMatrix;
         public:
             MatrixSelector(const MatrixDynSize& selectorMatrix)
                 : Selector(selectorMatrix.rows())
-                , m_selectorMatrix(selectorMatrix) {}
+            {
+                m_selectionMatrix = selectorMatrix;
+            }
 
             ~MatrixSelector() override;
 
             virtual const VectorDynSize& select(const VectorDynSize& fullVector) final
             {
-                iDynTree::toEigen(m_selected) = iDynTree::toEigen(m_selectorMatrix) * iDynTree::toEigen(fullVector);
+                iDynTree::toEigen(m_selected) = iDynTree::toEigen(m_selectionMatrix) * iDynTree::toEigen(fullVector);
                 return m_selected;
             }
         };
         MatrixSelector::~MatrixSelector(){}
 
-    //
-    // Implementation of TimeVaryingGradient
-    //
+        //
+        // End of implementation of TimeVaryingGradient
+        //
 
-        class TimeVaryingGradient : public TimeVaryingVector {
-            MatrixDynSize m_selectorMatrix, m_weightMatrix;
-            MatrixDynSize m_hessianMatrix, m_gradientSubMatrix;
-            std::shared_ptr<TimeVaryingVector> m_desiredTrajectory;
-            VectorDynSize m_outputVector;
+        class CostAttributes {
+            bool m_valid = false;
         public:
-            TimeVaryingGradient(const MatrixDynSize& selectorMatrix)
-            : m_selectorMatrix(selectorMatrix)
-            , m_desiredTrajectory(nullptr)
-            {
-                m_weightMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.rows());
-                toEigen(m_weightMatrix).setIdentity();
-                m_hessianMatrix.resize(m_selectorMatrix.cols(), m_selectorMatrix.cols());
-                toEigen(m_hessianMatrix) = toEigen(m_selectorMatrix).transpose() * toEigen(m_selectorMatrix);
-                m_gradientSubMatrix.resize(m_selectorMatrix.rows(), m_selectorMatrix.cols());
-                toEigen(m_gradientSubMatrix) = -1 * toEigen(m_selectorMatrix);
-                m_outputVector.resize(m_selectorMatrix.cols());
-                m_outputVector.zero();
+            std::shared_ptr<TimeVaryingVector> desiredTrajectory;
+            iDynTree::MatrixDynSize gradientSubMatrix, hessianMatrix;
+            iDynTree::MatrixDynSize weightMatrix;
+            std::shared_ptr<Selector> selector;
+            iDynTree::VectorDynSize buffer;
+
+            void initializeBuffers(unsigned int rangeSize, unsigned int totalDimension) {
+                this->buffer.resize(rangeSize);
+                this->buffer.zero();
+                this->desiredTrajectory = std::make_shared<TimeInvariantVector>(this->buffer);
+                this->weightMatrix.resize(rangeSize, rangeSize);
+                toEigen(this->weightMatrix).setIdentity();
+                this->gradientSubMatrix.resize(totalDimension, rangeSize);
+                toEigen(this->gradientSubMatrix) = toEigen(this->selector->asSelectorMatrix()).transpose();
+                this->hessianMatrix.resize(totalDimension, totalDimension);
+                toEigen(this->hessianMatrix) = toEigen(this->selector->asSelectorMatrix()).transpose() * toEigen(this->selector->asSelectorMatrix());
+                this->m_valid = true;
             }
 
-            ~TimeVaryingGradient() override;
+            void initializeCost(const IndexRange& selectedRange, unsigned int totalDimension) {
+                this->selector = std::make_shared<IndexSelector>(selectedRange, totalDimension);
+                unsigned int rangeSize = static_cast<unsigned int>(selectedRange.size);
+                initializeBuffers(rangeSize, totalDimension);
+            }
 
-            bool setDesiredTrajectory(std::shared_ptr<TimeVaryingVector> desiredTrajectory) {
-                if (!desiredTrajectory) {
-                    reportError("TimeVaryingGradient", "desiredTrajectory", "Empty desired trajectory pointer.");
-                    return false;
-                }
-                m_desiredTrajectory = desiredTrajectory;
+            void initializeCost(const MatrixDynSize& inputSelector) {
+                this->selector = std::make_shared<MatrixSelector>(inputSelector);
+                unsigned int rangeSize = inputSelector.rows();
+                unsigned int totalDimension = inputSelector.cols();
+                initializeBuffers(rangeSize, totalDimension);
+            }
+
+
+            bool setDesiredTrajectory(std::shared_ptr<TimeVaryingVector> inputTrajectory) {
+
+                this->desiredTrajectory = inputTrajectory;
                 return true;
             }
 
             bool setWeightMatrix(const MatrixDynSize &weights)
             {
                 if (weights.rows() != weights.cols()) {
-                    reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix is supposed to be squared.");
+                    reportError("L2NormCost", "setWeightMatrix", "The weights matrix is supposed to be squared.");
                     return false;
                 }
 
-                if (weights.cols() != m_selectorMatrix.rows()) {
-                    reportError("TimeVaryingGradient", "setWeightMatrix", "The weights matrix dimensions do not match those of the specified selector.");
+                if (weights.cols() != this->selector->size()) {
+                    reportError("L2NormCost", "setWeightMatrix", "The weights matrix dimensions do not match those of the specified selector.");
                     return false;
                 }
 
-                m_weightMatrix = weights;
-                toEigen(m_hessianMatrix) = toEigen(m_selectorMatrix).transpose() * toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
-                toEigen(m_gradientSubMatrix) = -0.5 * (toEigen(m_weightMatrix).transpose() * toEigen(m_selectorMatrix) + toEigen(m_weightMatrix) * toEigen(m_selectorMatrix));
+                this->weightMatrix = weights;
+                toEigen(this->gradientSubMatrix) = 0.5 * toEigen(this->selector->asSelectorMatrix()).transpose() * (toEigen(this->weightMatrix) + toEigen(this->weightMatrix).transpose());
+                toEigen(this->hessianMatrix) = toEigen(this->gradientSubMatrix) * toEigen(this->selector->asSelectorMatrix());
 
                 return true;
             }
 
-            virtual const VectorDynSize& get(double time, bool &isValid) override {
-                if (!m_desiredTrajectory) {
-                    isValid = true;
-                    return m_outputVector; //should be zero from the initialization
+            bool changeSelector(const MatrixDynSize& inputSelector) {
+                if ((inputSelector.rows() != this->selector->size()) || (inputSelector.cols() != this->selector->asSelectorMatrix().cols())) {
+                    reportError("L2NormCost", "getObject", "The new selector dimensionsions do not match the old one.");
+                    return false;
                 }
-                bool ok = false;
-                const VectorDynSize &desiredPoint = m_desiredTrajectory->get(time, ok);
+                this->selector.reset(new MatrixSelector(inputSelector));
+                toEigen(this->gradientSubMatrix) = 0.5 * toEigen(this->selector->asSelectorMatrix()).transpose() * (toEigen(this->weightMatrix) + toEigen(this->weightMatrix).transpose());
+                toEigen(this->hessianMatrix) = toEigen(this->gradientSubMatrix) * toEigen(this->selector->asSelectorMatrix());
+
+                return true;
+            }
+
+            bool isValid() {
+                return m_valid;
+            }
 
-                if (!ok) {
-                    isValid = false;
-                    m_outputVector.zero();
-                    return m_outputVector;
+            bool evaluateCost(double time, const VectorDynSize& values, double& costValue, const std::string &label) {
+                if (!isValid()) {
+                    costValue = 0.0;
+                    return true;
                 }
 
-                if (desiredPoint.size() != m_gradientSubMatrix.rows()) {
+                if (values.size() != selector->asSelectorMatrix().cols()) {
                     std::ostringstream errorMsg;
-                    errorMsg << "The specified desired point at time: " << time << " has size not matching the specified selector.";
-                    reportError("TimeVaryingGradient", "getObject", errorMsg.str().c_str());
-                    isValid = false;
-                    m_outputVector.zero();
-                    return m_outputVector;
+                    errorMsg << "The " << label <<" dimension is not the one expected from the specified selector.";
+                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
                 }
 
-                toEigen(m_outputVector) = toEigen(desiredPoint).transpose() * toEigen(m_gradientSubMatrix);
+                bool isValid;
+                const VectorDynSize& desired = desiredTrajectory->get(time, isValid);
 
-                isValid = true;
-                return m_outputVector;
-            }
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid desired " << label <<" at time: " << time << ".";
+                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
+                    return false;
+                }
 
-            bool updateSelector(const MatrixDynSize& selector) {
-                if ((selector.rows() != m_selectorMatrix.rows()) || (selector.cols() != m_selectorMatrix.cols())) {
-                    reportError("TimeVaryingGradient", "getObject", "The new selector dimensionsions do not match the old one.");
+                if (selector->size() != desired.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The desired " << label <<" dimension does not match the size of the selector at time: " << time << ".";
+                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
                     return false;
                 }
-                m_selectorMatrix = selector;
-                toEigen(m_hessianMatrix) = toEigen(m_selectorMatrix).transpose() * toEigen(m_weightMatrix) * toEigen(m_selectorMatrix);
-                toEigen(m_gradientSubMatrix) = -0.5 * (toEigen(m_weightMatrix).transpose() * toEigen(m_selectorMatrix) + toEigen(m_weightMatrix) * toEigen(m_selectorMatrix));
 
-                return true;
-            }
+                iDynTree::toEigen(buffer) = iDynTree::toEigen(selector->select(values)) - iDynTree::toEigen(desired);
 
-            const MatrixDynSize& selector() {
-                return m_selectorMatrix;
-            }
 
-            const MatrixDynSize& hessianMatrix() {
-                return m_hessianMatrix;
-            }
+                costValue = 0.5 * iDynTree::toEigen(buffer).transpose() * (iDynTree::toEigen(weightMatrix) * iDynTree::toEigen(buffer));
 
-            const MatrixDynSize& weightMatrix() {
-                return m_weightMatrix;
+                return true;
             }
 
-            std::shared_ptr<TimeVaryingVector> desiredTrajectory() {
-                return m_desiredTrajectory;
-            }
-        };
-        TimeVaryingGradient::~TimeVaryingGradient() {};
+            bool evaluateGradient(double time, const VectorDynSize& values, VectorDynSize& partialDerivative, const std::string &label) {
+                partialDerivative.resize(values.size());
 
-        //
-        // End of implementation of TimeVaryingGradient
-        //
+                if (!isValid()) {
+                    partialDerivative.zero();
+                    return true;
+                }
 
-        //
-        // Definition of pimpl
-        //
-        class L2NormCost::L2NormCostImplementation {
-            public:
-            std::shared_ptr<TimeVaryingGradient> stateGradient = nullptr, controlGradient = nullptr;
-            std::shared_ptr<TimeInvariantMatrix> stateHessian, controlHessian;
-            std::shared_ptr<Selector> stateSelector_ptr, controlSelector_ptr;
-            iDynTree::VectorDynSize stateCostBuffer, controlCostBuffer;
-
-            void initializeHessianAndGradient(const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector) {
-                if ((stateSelector.rows() != 0) && (stateSelector.cols() != 0)) {
-                    stateGradient = std::make_shared<TimeVaryingGradient>(stateSelector);
-                    stateHessian = std::make_shared<TimeInvariantMatrix>();
-                    stateHessian->get().resize(stateSelector.cols(), stateSelector.cols());
-                    stateHessian->get() = stateGradient->hessianMatrix();
-                    stateCostBuffer.resize(stateSelector.rows());
-                    stateCostBuffer.zero();
-                } else {
-                    stateGradient = nullptr;
+                if (values.size() != selector->asSelectorMatrix().cols()) {
+                    std::ostringstream errorMsg, errorTag;
+                    errorTag << "costFirstPartialDerivativeWRT" << label;
+                    errorMsg << "The " << label <<" dimension is not the one expected from the specified selector.";
+                    reportError("L2NormCost", errorTag.str().c_str(), errorMsg.str().c_str());
+                    return false;
                 }
 
-                if ((controlSelector.rows() != 0) && (controlSelector.cols() != 0)) {
-                    controlGradient = std::make_shared<TimeVaryingGradient>(controlSelector);
-                    controlHessian = std::make_shared<TimeInvariantMatrix>();
-                    controlHessian->get().resize(controlSelector.cols(), controlSelector.cols());
-                    controlHessian->get() = controlGradient->hessianMatrix();
-                    controlCostBuffer.resize(controlSelector.rows());
-                    controlCostBuffer.zero();
-                } else {
-                    controlGradient = nullptr;
+                bool isValid;
+                const VectorDynSize& desired = desiredTrajectory->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg, errorTag;
+                    errorTag << "costFirstPartialDerivativeWRT" << label;
+                    errorMsg << "Unable to retrieve a valid desired " << label <<" at time: " << time << ".";
+                    reportError("L2NormCost", errorTag.str().c_str(), errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (selector->size() != desired.size()) {
+                    std::ostringstream errorMsg, errorTag;
+                    errorTag << "costFirstPartialDerivativeWRT" << label;;
+                    errorMsg << "The desired " << label <<" dimension does not match the size of the selector at time: " << time << ".";
+                    reportError("L2NormCost", errorTag.str().c_str(), errorMsg.str().c_str());
+                    return false;
                 }
-            }
 
-            void initializeHessianAndGradient(unsigned int stateDimension, unsigned int controlDimension) {
-                MatrixDynSize stateSelector(stateDimension, stateDimension), controlSelector(controlDimension, controlDimension);
-                toEigen(stateSelector).setIdentity();
-                toEigen(controlSelector).setIdentity();
-                initializeHessianAndGradient(stateSelector, controlSelector);
+                iDynTree::toEigen(buffer) = iDynTree::toEigen(selector->select(values)) - iDynTree::toEigen(desired);
+                iDynTree::toEigen(partialDerivative) = iDynTree::toEigen(gradientSubMatrix) * iDynTree::toEigen(buffer);
+
+                return true;
             }
 
-            void initializeHessianAndGradient(const IndexRange &stateSelector, unsigned int totalStateDimension, const IndexRange &controlSelector, unsigned int totalControlDimension) {
-                MatrixDynSize stateSelectorMatrix, controlSelectorMatrix;
+            bool getHessian(const VectorDynSize& values, MatrixDynSize& partialDerivative, const std::string &label) {
 
-                if (stateSelector.isValid()) {
-                    stateSelectorMatrix.resize(static_cast<unsigned int>(stateSelector.size), totalStateDimension);
-                    iDynTree::toEigen(stateSelectorMatrix).block(0, stateSelector.offset, stateSelector.size, stateSelector.size).setIdentity();
-                } else {
-                    stateSelectorMatrix.resize(0,0);
+                partialDerivative.resize(values.size(), values.size());
+
+                if (!isValid()) {
+                    partialDerivative.zero();
+                    return true;
                 }
 
-                if (controlSelector.isValid()) {
-                    controlSelectorMatrix.resize(static_cast<unsigned int>(controlSelector.size), totalControlDimension);
-                    iDynTree::toEigen(controlSelectorMatrix).block(0, controlSelector.offset, controlSelector.size, controlSelector.size).setIdentity();
-                } else {
-                    controlSelectorMatrix.resize(0,0);
+                if (values.size() != selector->asSelectorMatrix().cols()) {
+                    std::ostringstream errorMsg, errorTag;
+                    errorTag << "costSecondPartialDerivativeWRT" << label;
+                    errorMsg << "The " << label <<" dimension is not the one expected from the specified selector.";
+                    reportError("L2NormCost", errorTag.str().c_str(), errorMsg.str().c_str());
+                    return false;
                 }
 
-                initializeHessianAndGradient(stateSelectorMatrix, controlSelectorMatrix);
+                partialDerivative = hessianMatrix;
+
+                return true;
+
             }
 
         };
 
+        //
+        // Definition of pimpl
+        //
+        class L2NormCost::L2NormCostImplementation {
+            public:
+            CostAttributes stateCost, controlCost;
+        };
+
         //
         //End of definition of pimpl
         //
@@ -269,67 +292,51 @@ namespace iDynTree {
         //
 
         L2NormCost::L2NormCost(const std::string &name, unsigned int stateDimension, unsigned int controlDimension)
-        : QuadraticLikeCost(name)
-        , m_pimpl(new L2NormCostImplementation)
+            : Cost(name)
+            , m_pimpl(new L2NormCostImplementation)
         {
-            assert(m_pimpl);
-
-            m_pimpl->initializeHessianAndGradient(stateDimension, controlDimension);
-
-            if (m_pimpl->stateGradient) {
-                m_timeVaryingStateHessian = m_pimpl->stateHessian;
-                m_timeVaryingStateGradient = m_pimpl->stateGradient;
+            if (stateDimension > 0) {
                 IndexRange stateRange;
                 stateRange.offset = 0;
                 stateRange.size = stateDimension;
-                m_pimpl->stateSelector_ptr = std::make_shared<IndexSelector>(stateRange);
+
+                m_pimpl->stateCost.initializeCost(stateRange, stateDimension);
             }
 
-            if (m_pimpl->controlGradient) {
-                m_timeVaryingControlHessian = m_pimpl->controlHessian;
-                m_timeVaryingControlGradient = m_pimpl->controlGradient;
+            if (controlDimension > 0) {
                 IndexRange controlRange;
                 controlRange.offset = 0;
-                controlRange.size = stateDimension;
-                m_pimpl->controlSelector_ptr = std::make_shared<IndexSelector>(controlRange);
+                controlRange.size = controlDimension;
+
+                m_pimpl->controlCost.initializeCost(controlRange, controlDimension);
             }
         }
 
         L2NormCost::L2NormCost(const std::string &name, const MatrixDynSize &stateSelector, const MatrixDynSize &controlSelector)
-        : QuadraticLikeCost(name)
-        , m_pimpl(new L2NormCostImplementation)
+            : Cost(name)
+            , m_pimpl(new L2NormCostImplementation)
         {
-            m_pimpl->initializeHessianAndGradient(stateSelector, controlSelector);
-
-            if (m_pimpl->stateGradient) {
-                m_timeVaryingStateHessian = m_pimpl->stateHessian;
-                m_timeVaryingStateGradient = m_pimpl->stateGradient;
-                m_pimpl->stateSelector_ptr = std::make_shared<MatrixSelector>(stateSelector);
+            if ((stateSelector.rows() != 0) && (stateSelector.cols() != 0)) {
+                m_pimpl->stateCost.initializeCost(stateSelector);
             }
 
-            if (m_pimpl->controlGradient) {
-                m_timeVaryingControlHessian = m_pimpl->controlHessian;
-                m_timeVaryingControlGradient = m_pimpl->controlGradient;
-                m_pimpl->controlSelector_ptr = std::make_shared<MatrixSelector>(controlSelector);
+            if ((controlSelector.rows() != 0) && (controlSelector.cols() != 0)) {
+                m_pimpl->controlCost.initializeCost(controlSelector);
             }
         }
 
         L2NormCost::L2NormCost(const std::string &name, const IndexRange &stateSelector, unsigned int totalStateDimension, const IndexRange &controlSelector, unsigned int totalControlDimension)
-            : QuadraticLikeCost (name)
+            : Cost (name)
             , m_pimpl(new L2NormCostImplementation)
         {
-            m_pimpl->initializeHessianAndGradient(stateSelector, totalStateDimension, controlSelector, totalControlDimension);
+            if (stateSelector.isValid() && totalStateDimension > 0) {
+                m_pimpl->stateCost.initializeCost(stateSelector, totalStateDimension);
 
-            if (m_pimpl->stateGradient) {
-                m_timeVaryingStateHessian = m_pimpl->stateHessian;
-                m_timeVaryingStateGradient = m_pimpl->stateGradient;
-                m_pimpl->stateSelector_ptr = std::make_shared<IndexSelector>(stateSelector);
             }
 
-            if (m_pimpl->controlGradient) {
-                m_timeVaryingControlHessian = m_pimpl->controlHessian;
-                m_timeVaryingControlGradient = m_pimpl->controlGradient;
-                m_pimpl->controlSelector_ptr = std::make_shared<IndexSelector>(controlSelector);
+            if (controlSelector.isValid() && totalControlDimension > 0) {
+                m_pimpl->controlCost.initializeCost(controlSelector, totalControlDimension);
+
             }
         }
 
@@ -348,12 +355,12 @@ namespace iDynTree {
                 return false;
             }
 
-            if (!(m_pimpl->stateGradient)) {
+            if (!(m_pimpl->stateCost.isValid())) {
                 reportError("L2NormCost", "setStateWeight", "The state cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
 
-            if (!(m_pimpl->stateGradient->setWeightMatrix(stateWeights))) {
+            if (!(m_pimpl->stateCost.setWeightMatrix(stateWeights))) {
                 reportError("L2NormCost", "setStateWeight", "Error when specifying the state weights.");
                 return false;
             }
@@ -363,23 +370,23 @@ namespace iDynTree {
 
         bool L2NormCost::setStateDesiredPoint(const VectorDynSize &desiredPoint)
         {
-            if (!(m_pimpl->stateGradient)) {
+            if (!(m_pimpl->stateCost.isValid())) {
                 reportError("L2NormCost", "setStateDesiredPoint", "The state cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
 
-            if (desiredPoint.size() != (m_pimpl->stateGradient->selector().rows())) {
+            if (desiredPoint.size() != (m_pimpl->stateCost.selector->size())) {
                 reportError("L2NormCost", "setStateDesiredPoint", "The desiredPoint size do not match the dimension of the specified selector.");
                 return false;
             }
 
             std::shared_ptr<TimeInvariantVector> newTrajectory = std::make_shared<TimeInvariantVector>(desiredPoint);
-            return m_pimpl->stateGradient->setDesiredTrajectory(newTrajectory);
+            return m_pimpl->stateCost.setDesiredTrajectory(newTrajectory);
         }
 
         bool L2NormCost::setStateDesiredTrajectory(std::shared_ptr<TimeVaryingVector> stateDesiredTrajectory)
         {
-            if (!(m_pimpl->stateGradient)) {
+            if (!(m_pimpl->stateCost.isValid())) {
                 reportError("L2NormCost", "setStateDesiredTrajectory", "The state cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
@@ -389,7 +396,7 @@ namespace iDynTree {
                 return false;
             }
 
-            return m_pimpl->stateGradient->setDesiredTrajectory(stateDesiredTrajectory);
+            return m_pimpl->stateCost.setDesiredTrajectory(stateDesiredTrajectory);
         }
 
         bool L2NormCost::setControlWeight(const MatrixDynSize &controlWeights)
@@ -399,12 +406,12 @@ namespace iDynTree {
                 return false;
             }
 
-            if (!(m_pimpl->controlGradient)) {
+            if (!(m_pimpl->stateCost.isValid())) {
                 reportError("L2NormCost", "setControlWeight", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
 
-            if (!(m_pimpl->controlGradient->setWeightMatrix(controlWeights))) {
+            if (!(m_pimpl->controlCost.setWeightMatrix(controlWeights))) {
                 reportError("L2NormCost", "setControlWeight", "Error when specifying the control weights.");
                 return false;
             }
@@ -414,23 +421,23 @@ namespace iDynTree {
 
         bool L2NormCost::setControlDesiredPoint(const VectorDynSize &desiredPoint)
         {
-            if (!(m_pimpl->controlGradient)) {
+            if (!(m_pimpl->stateCost.isValid())) {
                 reportError("L2NormCost", "setControlDesiredPoint", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
 
-            if (desiredPoint.size() != (m_pimpl->controlGradient->selector().rows())) {
+            if (desiredPoint.size() != (m_pimpl->controlCost.selector->size())) {
                 reportError("L2NormCost", "setControlDesiredPoint", "The desiredPoint size do not match the dimension of the specified selector.");
                 return false;
             }
 
             std::shared_ptr<TimeInvariantVector> newTrajectory = std::make_shared<TimeInvariantVector>(desiredPoint);
-            return m_pimpl->controlGradient->setDesiredTrajectory(newTrajectory);
+            return m_pimpl->controlCost.setDesiredTrajectory(newTrajectory);
         }
 
         bool L2NormCost::setControlDesiredTrajectory(std::shared_ptr<TimeVaryingVector> controlDesiredTrajectory)
         {
-            if (!(m_pimpl->controlGradient)) {
+            if (!(m_pimpl->stateCost.isValid())) {
                 reportError("L2NormCost", "setControlDesiredTrajectory", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
@@ -440,26 +447,24 @@ namespace iDynTree {
                 return false;
             }
 
-            return m_pimpl->controlGradient->setDesiredTrajectory(controlDesiredTrajectory);
+            return m_pimpl->controlCost.setDesiredTrajectory(controlDesiredTrajectory);
         }
 
         bool L2NormCost::updatStateSelector(const MatrixDynSize &stateSelector)
         {
-            if (!(m_pimpl->stateGradient->updateSelector(stateSelector))) {
+            if (!(m_pimpl->stateCost.changeSelector(stateSelector))) {
                 reportError("L2NormCost", "updatStateSelector", "Error when updating state selector.");
                 return false;
             }
-            m_pimpl->stateHessian->get() = m_pimpl->stateGradient->hessianMatrix();
             return true;
         }
 
         bool L2NormCost::updatControlSelector(const MatrixDynSize &controlSelector)
         {
-            if (!(m_pimpl->controlGradient->updateSelector(controlSelector))) {
+            if (!(m_pimpl->controlCost.changeSelector(controlSelector))) {
                 reportError("L2NormCost", "updatControlSelector", "Error when updating state selector.");
                 return false;
             }
-            m_pimpl->controlHessian->get() = m_pimpl->controlGradient->hessianMatrix();
             return true;
         }
 
@@ -467,77 +472,41 @@ namespace iDynTree {
         {
             double stateCost = 0, controlCost = 0;
 
-            bool isValid = false;
-            if (m_pimpl->stateGradient) {
-
-                if (state.size() != m_pimpl->stateGradient->selector().cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The state dimension is not the one expected from the specified selector.";
-                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
-
-                iDynTree::toEigen(m_pimpl->stateCostBuffer) = iDynTree::toEigen(m_pimpl->stateSelector_ptr->select(state));
-
-                if (m_pimpl->stateGradient->desiredTrajectory()) {
-                    const VectorDynSize& desired = m_pimpl->stateGradient->desiredTrajectory()->get(time, isValid);
-
-                    if (!isValid) {
-                        std::ostringstream errorMsg;
-                        errorMsg << "Unable to retrieve a valid desired state at time: " << time << ".";
-                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
-                        return false;
-                    }
-
-                    if (m_pimpl->stateSelector_ptr->size() != desired.size()) {
-                        std::ostringstream errorMsg;
-                        errorMsg << "The desired state dimension does not match the size of the selector at time: " << time << ".";
-                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
-                        return false;
-                    }
-
-                    iDynTree::toEigen(m_pimpl->stateCostBuffer) -= iDynTree::toEigen(desired);
-                }
-
-                stateCost = 0.5 * iDynTree::toEigen(m_pimpl->stateCostBuffer).transpose() * (iDynTree::toEigen(m_pimpl->stateGradient->weightMatrix()) * iDynTree::toEigen(m_pimpl->stateCostBuffer));
-            }
-
-            if (m_pimpl->controlGradient) {
-
-                if (control.size() != m_pimpl->controlGradient->selector().cols()) {
-                    std::ostringstream errorMsg;
-                    errorMsg << "The control dimension is not the one expected from the specified selector.";
-                    reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
-                    return false;
-                }
+            if (!m_pimpl->stateCost.evaluateCost(time, state, stateCost, "state"))
+                return false;
 
-                iDynTree::toEigen(m_pimpl->controlCostBuffer) = iDynTree::toEigen(m_pimpl->controlSelector_ptr->select(control));
+            if (!m_pimpl->controlCost.evaluateCost(time, control, controlCost, "control"))
+                return false;
 
-                if (m_pimpl->controlGradient->desiredTrajectory()) {
-                    const VectorDynSize& desired = m_pimpl->controlGradient->desiredTrajectory()->get(time, isValid);
+            costValue = stateCost + controlCost;
 
-                    if (!isValid) {
-                        std::ostringstream errorMsg;
-                        errorMsg << "Unable to retrieve a valid desired control at time: " << time << ".";
-                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
-                        return false;
-                    }
+            return true;
+        }
 
-                    if (m_pimpl->controlSelector_ptr->size() != desired.size()) {
-                        std::ostringstream errorMsg;
-                        errorMsg << "The desired control dimension does not match the size of the selector at time: " << time << ".";
-                        reportError("L2NormCost", "costEvaluation", errorMsg.str().c_str());
-                        return false;
-                    }
+        bool L2NormCost::costFirstPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &/*control*/, VectorDynSize &partialDerivative)
+        {
+            return m_pimpl->stateCost.evaluateGradient(time, state, partialDerivative, "state");
+        }
 
-                    iDynTree::toEigen(m_pimpl->controlCostBuffer) -= iDynTree::toEigen(desired);
-                }
+        bool L2NormCost::costFirstPartialDerivativeWRTControl(double time, const VectorDynSize &/*state*/, const VectorDynSize &control, VectorDynSize &partialDerivative)
+        {
+            return m_pimpl->controlCost.evaluateGradient(time, control, partialDerivative, "control");
+        }
 
-                controlCost = 0.5 * iDynTree::toEigen(m_pimpl->controlCostBuffer).transpose() * (iDynTree::toEigen(m_pimpl->controlGradient->weightMatrix()) * iDynTree::toEigen(m_pimpl->controlCostBuffer));
-            }
+        bool L2NormCost::costSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &state, const VectorDynSize &/*control*/, MatrixDynSize &partialDerivative)
+        {
+            return m_pimpl->stateCost.getHessian(state, partialDerivative, "state");
+        }
 
-            costValue = stateCost + controlCost;
+        bool L2NormCost::costSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &control, MatrixDynSize &partialDerivative)
+        {
+            return m_pimpl->controlCost.getHessian(control, partialDerivative, "control");
+        }
 
+        bool L2NormCost::costSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &partialDerivative)
+        {
+            partialDerivative.resize(state.size(), control.size());
+            partialDerivative.zero();
             return true;
         }
 
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
index 497f3333f54..95df3e3f476 100644
--- a/src/optimalcontrol/src/QuadraticLikeCost.cpp
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -210,7 +210,7 @@ namespace iDynTree {
                     reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTState", errorMsg.str().c_str());
                     return false;
                 }
-                toEigen(partialDerivative) += toEigen(hessian) * toEigen(state);
+                toEigen(partialDerivative) += 0.5 * (toEigen(hessian) + toEigen(hessian).transpose()) * toEigen(state);
             }
 
             if (m_timeVaryingStateGradient) {
@@ -271,7 +271,7 @@ namespace iDynTree {
                     reportError("QuadraticLikeCost", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
                     return false;
                 }
-                toEigen(partialDerivative) += toEigen(hessian) * toEigen(control);
+                toEigen(partialDerivative) += 0.5 * (toEigen(hessian) + toEigen(hessian).transpose()) * toEigen(control);
             }
 
             if (m_timeVaryingControlGradient) {
@@ -333,7 +333,7 @@ namespace iDynTree {
                     return false;
                 }
 
-                partialDerivative = hessian;
+                toEigen(partialDerivative) = 0.5 * (toEigen(hessian) + toEigen(hessian).transpose());
 
             } else {
 
@@ -376,7 +376,7 @@ namespace iDynTree {
                     return false;
                 }
 
-                partialDerivative = hessian;
+                toEigen(partialDerivative) = 0.5 * (toEigen(hessian) + toEigen(hessian).transpose());
             } else {
                 partialDerivative.zero();
             }

From 6ef3365ee389cad3f598487b600d9a045c13e946 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 31 Aug 2018 09:32:23 +0200
Subject: [PATCH 042/194] Added other types of TimeVaryingObjects.

---
 .../include/iDynTree/TimeVaryingObject.h      | 20 +++++++++++++++++++
 src/optimalcontrol/src/TimeVaryingObject.cpp  |  5 +++++
 2 files changed, 25 insertions(+)

diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
index 5a6b1d861e8..b0cc1f761cd 100644
--- a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
+++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
@@ -20,6 +20,7 @@
 
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/Transform.h>
 
 namespace iDynTree {
 
@@ -45,6 +46,13 @@ namespace iDynTree {
 
         typedef TimeVaryingObject<double> TimeVaryingDouble;
 
+        typedef TimeVaryingObject<Transform> TimeVaryingTransform;
+
+        typedef TimeVaryingObject<Rotation> TimeVaryingRotation;
+
+        typedef TimeVaryingObject<Position> TimeVaryingPosition;
+
+
         /**
          * @warning This class is still in active development, and so API interface can change between iDynTree versions.
          * \ingroup iDynTreeExperimental
@@ -69,11 +77,23 @@ namespace iDynTree {
 
         extern template class TimeInvariantObject<MatrixDynSize>;
 
+        extern template class TimeInvariantObject<Transform>;
+
+        extern template class TimeInvariantObject<Rotation>;
+
+        extern template class TimeInvariantObject<Position>;
+
         typedef TimeInvariantObject<double> TimeInvariantDouble;
 
         typedef TimeInvariantObject<VectorDynSize> TimeInvariantVector;
 
         typedef TimeInvariantObject<MatrixDynSize> TimeInvariantMatrix;
+
+        typedef TimeInvariantObject<Transform> TimeInvariantTransform;
+
+        typedef TimeInvariantObject<Rotation> TimeInvariantRotation;
+
+        typedef TimeInvariantObject<Position> TimeInvariantPosition;
     }
 }
 
diff --git a/src/optimalcontrol/src/TimeVaryingObject.cpp b/src/optimalcontrol/src/TimeVaryingObject.cpp
index 5e426eb31a6..76a729c95db 100644
--- a/src/optimalcontrol/src/TimeVaryingObject.cpp
+++ b/src/optimalcontrol/src/TimeVaryingObject.cpp
@@ -47,5 +47,10 @@ namespace iDynTree {
 
         template class TimeInvariantObject<MatrixDynSize>;
 
+        template class TimeInvariantObject<Transform>;
+
+        template class TimeInvariantObject<Rotation>;
+
+        template class TimeInvariantObject<Position>;
     }
 }

From 4a0ed677a5a15b4a8c8fa8bbbb568d6cbced080f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 13 Sep 2018 18:45:00 +0200
Subject: [PATCH 043/194] Better printing in ASSERT_EQUAL_VECTOR.

Avoiding overriding QuadraticLikeCost methods.
---
 src/core/include/iDynTree/Core/TestUtils.h              | 1 +
 src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/core/include/iDynTree/Core/TestUtils.h b/src/core/include/iDynTree/Core/TestUtils.h
index f32e3bfb1b5..672fd2791e8 100644
--- a/src/core/include/iDynTree/Core/TestUtils.h
+++ b/src/core/include/iDynTree/Core/TestUtils.h
@@ -179,6 +179,7 @@ namespace iDynTree
         {
             std::cerr << vec(i) << "\n";
         }
+        std::cerr << "\n";
     }
 
     /**
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
index 27eaf08167f..05e6c5bbfbd 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
@@ -39,7 +39,7 @@ namespace iDynTree {
            virtual bool costEvaluation(double time,
                                        const iDynTree::VectorDynSize& state,
                                        const iDynTree::VectorDynSize& control,
-                                       double& costValue) override;//0.5 xT H x + gx + c
+                                       double& costValue) final;//0.5 xT H x + gx + c
 
            virtual bool costFirstPartialDerivativeWRTState(double time,
                                                            const iDynTree::VectorDynSize& state,

From 61fa0c23dc5198376ffe2ff4e3e22d490d4af364 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 13 Sep 2018 18:46:14 +0200
Subject: [PATCH 044/194] The interfaces to the solvers are always compiled but
 with different implementations.

---
 src/optimalcontrol/CMakeLists.txt             |   8 +-
 .../include/iDynTree/Optimizer.h              |   2 +
 .../iDynTree/Optimizers/IpoptInterface.h      |   2 +
 .../iDynTree/Optimizers/OsqpInterface.h       |   2 +
 src/optimalcontrol/src/IpoptInterface.cpp     |  29 +++-
 .../src/IpoptInterfaceNotImplemented.cpp      | 127 ++++++++++++++++++
 src/optimalcontrol/src/OsqpInterface.cpp      |   7 +-
 .../src/OsqpInterfaceNotImplemented.cpp       |  95 +++++++++++++
 .../tests/MultipleShootingTest.cpp            |   4 +
 9 files changed, 267 insertions(+), 9 deletions(-)
 create mode 100644 src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
 create mode 100644 src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp

diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 4d153353658..bfcadef696c 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -73,17 +73,21 @@ set(SOURCES src/DynamicalSystem.cpp
             src/ForwardEuler.cpp
             src/TimeVaryingObject.cpp)
 
+list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/IpoptInterface.h)
 if (IDYNTREE_USES_IPOPT)
-    list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/IpoptInterface.h)
     list(APPEND OPTIMIZERS_SOURCES src/IpoptInterface.cpp)
     list(APPEND LINK_LIST ${IPOPT_LIBRARIES})
     list(APPEND INCLUDE_LIST ${IPOPT_INCLUDE_DIRS})
+else()
+    list(APPEND OPTIMIZERS_SOURCES src/IpoptInterfaceNotImplemented.cpp)
 endif()
 
+list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/OsqpInterface.h)
 if (IDYNTREE_USES_OSQPEIGEN)
-    list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/OsqpInterface.h)
     list(APPEND OPTIMIZERS_SOURCES src/OsqpInterface.cpp)
     list(APPEND LINK_LIST   OsqpEigen::OsqpEigen osqp::osqp)
+else()
+    list(APPEND OPTIMIZERS_SOURCES src/OsqpInterfaceNotImplemented.cpp)
 endif()
 
 
diff --git a/src/optimalcontrol/include/iDynTree/Optimizer.h b/src/optimalcontrol/include/iDynTree/Optimizer.h
index d10ea1e0fa6..1a56df70b44 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizer.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizer.h
@@ -42,6 +42,8 @@ namespace iDynTree {
 
             virtual ~Optimizer();
 
+            virtual bool isAvailable() = 0; //is the desired interface implemented?
+
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem);
 
             virtual const std::weak_ptr<OptimizationProblem> problem() const;
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
index ba93f607bf0..77a6752343a 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
@@ -45,6 +45,8 @@ namespace iDynTree {
 
             virtual ~IpoptInterface() override;
 
+            virtual bool isAvailable() override;
+
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
 
             virtual bool setInitialGuess(VectorDynSize &initialGuess) override;
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
index feb7b024bc7..1462a7a20a6 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
@@ -70,6 +70,8 @@ namespace iDynTree {
 
             virtual ~OsqpInterface() override;
 
+            virtual bool isAvailable() override;
+
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
 
             virtual bool setInitialGuess(VectorDynSize &initialGuess) override;
diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index 7f9807fbd77..10f179c1af5 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -13,12 +13,24 @@
  * - ACADO toolbox (http://acado.github.io)
  * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
  */
-
-#define HAVE_STDDEF_H
-#define HAVE_CSTDDEF
-#include <IpTNLP.hpp>
-#undef HAVE_STDDEF_H
-#undef HAVE_CSTDDEF //workaroud for missing libraries
+#ifndef HAVE_STDDEF_H
+    #ifndef HAVE_CSTDDEF
+        #define HAVE_CSTDDEF
+        #include <IpTNLP.hpp>
+        #undef HAVE_CSTDDEF
+    #else
+        #include <IpTNLP.hpp>
+    #endif
+    #undef HAVE_STDDEF_H
+#else
+    #ifndef HAVE_CSTDDEF
+        #define HAVE_CSTDDEF
+        #include <IpTNLP.hpp>
+        #undef HAVE_CSTDDEF
+    #else
+        #include <IpTNLP.hpp>
+    #endif
+#endif
 #include <IpIpoptApplication.hpp>
 
 #include <Eigen/Dense>
@@ -483,6 +495,11 @@ namespace iDynTree {
             }
         }
 
+        bool IpoptInterface::isAvailable()
+        {
+            return true;
+        }
+
         bool IpoptInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
         {
             if (!problem){
diff --git a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
new file mode 100644
index 00000000000..2227cae4ad0
--- /dev/null
+++ b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
@@ -0,0 +1,127 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/IpoptInterface.h>
+#include <iDynTree/Core/Utils.h>
+
+using namespace iDynTree::optimization;
+
+IpoptInterface::IpoptInterface()
+{
+
+}
+
+IpoptInterface::~IpoptInterface()
+{
+
+}
+
+bool IpoptInterface::isAvailable()
+{
+    return false;
+}
+
+bool IpoptInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+{
+    reportError("IpoptInterface", "setProblem", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::setInitialGuess(VectorDynSize &initialGuess)
+{
+    reportError("IpoptInterface", "setInitialGuess", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::solve()
+{
+    reportError("IpoptInterface", "solve", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getPrimalVariables(VectorDynSize &primalVariables)
+{
+    reportError("IpoptInterface", "getPrimalVariables", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
+{
+    reportError("IpoptInterface", "getDualVariables", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getOptimalCost(double &optimalCost)
+{
+    reportError("IpoptInterface", "getOptimalCost", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
+{
+    reportError("IpoptInterface", "getOptimalConstraintsValues", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+double IpoptInterface::minusInfinity()
+{
+    reportError("IpoptInterface", "minusInfinity", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return Optimizer::minusInfinity();
+}
+
+double IpoptInterface::plusInfinity()
+{
+    reportError("IpoptInterface", "plusInfinity", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return Optimizer::plusInfinity();
+}
+
+bool IpoptInterface::setIpoptOption(const std::string &tag, const std::string &value)
+{
+    reportError("IpoptInterface", "setIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::setIpoptOption(const std::string &tag, double value)
+{
+    reportError("IpoptInterface", "setIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::setIpoptOption(const std::string &tag, int value)
+{
+    reportError("IpoptInterface", "setIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getIpoptOption(const std::string &tag, std::string &value)
+{
+    reportError("IpoptInterface", "getIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getIpoptOption(const std::string &tag, double &value)
+{
+    reportError("IpoptInterface", "getIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool IpoptInterface::getIpoptOption(const std::string &tag, int &value)
+{
+    reportError("IpoptInterface", "getIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+
diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index 2e503376341..9cff7f9aecd 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -437,6 +437,11 @@ namespace iDynTree {
             }
         }
 
+        bool OsqpInterface::isAvailable()
+        {
+            return true;
+        }
+
         bool OsqpInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
         {
             if (!problem){
@@ -529,7 +534,7 @@ namespace iDynTree {
             if (m_pimpl->hasBoxConstraints) {
                 m_pimpl->unifiedLowerBounds.head(m_pimpl->nv) = toEigen(m_pimpl->variablesLowerBounds);
                 m_pimpl->unifiedLowerBounds.tail(m_problem->numberOfConstraints()) =
-                        toEigen(m_pimpl->constraintsLowerBounds) - toEigen(m_pimpl->constraintsBuffer); //remove eventual bias in the constraints
+                        toEigen(m_pimpl->constraintsLowerBounds) - toEigen(m_pimpl->constraintsBuffer); //remove eventual bias in the constraints. Infact, suppose that the constraint is written as Ax - b = 0, it is not enough to consider only the constraint jacobian A.
 
                 m_pimpl->unifiedUpperBounds.head(m_pimpl->nv) = toEigen(m_pimpl->variablesUpperBounds);
                 m_pimpl->unifiedUpperBounds.tail(m_problem->numberOfConstraints()) =
diff --git a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
new file mode 100644
index 00000000000..e4b0ee13bc9
--- /dev/null
+++ b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/OsqpInterface.h>
+#include <iDynTree/Core/Utils.h>
+
+
+using namespace iDynTree::optimization;
+
+OsqpInterface::OsqpInterface()
+{ }
+
+OsqpInterface::~OsqpInterface()
+{ }
+
+bool OsqpInterface::isAvailable()
+{
+    return false;
+}
+
+bool OsqpInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+{
+    reportError("OsqpInterface", "setProblem", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+bool OsqpInterface::setInitialGuess(VectorDynSize &initialGuess)
+{
+    reportError("OsqpInterface", "setInitialGuess", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+bool OsqpInterface::solve()
+{
+    reportError("OsqpInterface", "solve", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+bool OsqpInterface::getPrimalVariables(VectorDynSize &primalVariables)
+{
+    reportError("OsqpInterface", "getPrimalVariables", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+bool OsqpInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
+{
+    reportError("OsqpInterface", "getDualVariables", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+bool OsqpInterface::getOptimalCost(double &optimalCost)
+{
+    reportError("OsqpInterface", "getOptimalCost", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+bool OsqpInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
+{
+    reportError("OsqpInterface", "getOptimalConstraintsValues", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return false;
+}
+
+double OsqpInterface::minusInfinity()
+{
+    reportError("OsqpInterface", "setProblem", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return Optimizer::minusInfinity();
+}
+
+double OsqpInterface::plusInfinity()
+{
+    reportError("OsqpInterface", "setProblem", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    return Optimizer::plusInfinity();
+}
+
+OsqpSettings &OsqpInterface::settings()
+{
+    reportError("OsqpInterface", "settings", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
+    OsqpSettings settings;
+    return settings;
+}
+
+
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 9fe621a4152..8b1e913d621 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -265,6 +265,10 @@ class OptimizerTest : public iDynTree::optimization::Optimizer {
 
     virtual ~OptimizerTest() override {}
 
+    virtual bool isAvailable() override{
+        return true;
+    }
+
     virtual bool setInitialGuess(iDynTree::VectorDynSize &initialGuess) override{
         return true;
     }

From e0fdd3969d47d8289e409a750ac5a52812626f92 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 14 Sep 2018 17:09:00 +0200
Subject: [PATCH 045/194] Fixed bug in vector map in EigenHelper.

---
 src/core/include/iDynTree/Core/EigenHelpers.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/core/include/iDynTree/Core/EigenHelpers.h b/src/core/include/iDynTree/Core/EigenHelpers.h
index 76f7f42bcdc..210d126c7c1 100644
--- a/src/core/include/iDynTree/Core/EigenHelpers.h
+++ b/src/core/include/iDynTree/Core/EigenHelpers.h
@@ -31,7 +31,7 @@ namespace iDynTree
     //Useful typedefs
     //TODO: change methods below to use these typedefs
     typedef Eigen::Map<Eigen::VectorXd> iDynTreeEigenVector;
-    typedef Eigen::Map<Eigen::VectorXd> iDynTreeEigenConstVector;
+    typedef Eigen::Map<const Eigen::VectorXd> iDynTreeEigenConstVector;
     typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> iDynTreeEigenMatrix;
     typedef const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> iDynTreeEigenConstMatrix;
     typedef Eigen::Map<iDynTreeEigenMatrix> iDynTreeEigenMatrixMap;

From 64c1c43e0f037b7b4fdd273d58aca673a6b817ca Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 14 Sep 2018 17:10:52 +0200
Subject: [PATCH 046/194] The isAvailable method is const.

---
 src/optimalcontrol/include/iDynTree/Optimizer.h                 | 2 +-
 src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h | 2 +-
 src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h  | 2 +-
 src/optimalcontrol/src/IpoptInterface.cpp                       | 2 +-
 src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp         | 2 +-
 src/optimalcontrol/src/OsqpInterface.cpp                        | 2 +-
 src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp          | 2 +-
 7 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/Optimizer.h b/src/optimalcontrol/include/iDynTree/Optimizer.h
index 1a56df70b44..6f9d8817096 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizer.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizer.h
@@ -42,7 +42,7 @@ namespace iDynTree {
 
             virtual ~Optimizer();
 
-            virtual bool isAvailable() = 0; //is the desired interface implemented?
+            virtual bool isAvailable() const = 0; //is the desired interface implemented?
 
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem);
 
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
index 77a6752343a..07455c279b9 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
@@ -45,7 +45,7 @@ namespace iDynTree {
 
             virtual ~IpoptInterface() override;
 
-            virtual bool isAvailable() override;
+            virtual bool isAvailable() const override;
 
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
 
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
index 1462a7a20a6..d05c08df77f 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
@@ -70,7 +70,7 @@ namespace iDynTree {
 
             virtual ~OsqpInterface() override;
 
-            virtual bool isAvailable() override;
+            virtual bool isAvailable() const override;
 
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
 
diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index 10f179c1af5..00b157901dd 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -495,7 +495,7 @@ namespace iDynTree {
             }
         }
 
-        bool IpoptInterface::isAvailable()
+        bool IpoptInterface::isAvailable() const
         {
             return true;
         }
diff --git a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
index 2227cae4ad0..9c9ddb3d709 100644
--- a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
@@ -29,7 +29,7 @@ IpoptInterface::~IpoptInterface()
 
 }
 
-bool IpoptInterface::isAvailable()
+bool IpoptInterface::isAvailable() const
 {
     return false;
 }
diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index 9cff7f9aecd..b5688e562b3 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -437,7 +437,7 @@ namespace iDynTree {
             }
         }
 
-        bool OsqpInterface::isAvailable()
+        bool OsqpInterface::isAvailable() const
         {
             return true;
         }
diff --git a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
index e4b0ee13bc9..126bea7ae22 100644
--- a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
@@ -26,7 +26,7 @@ OsqpInterface::OsqpInterface()
 OsqpInterface::~OsqpInterface()
 { }
 
-bool OsqpInterface::isAvailable()
+bool OsqpInterface::isAvailable() const
 {
     return false;
 }

From 85e4b64aacf2c40b0440eb2718f359097fb53cbe Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 14 Sep 2018 17:11:19 +0200
Subject: [PATCH 047/194] Added utility functions.

Get timings from multiple shooting solver without warnings before computing a solution. The weights In L2NormCost can be set from a vector.
---
 .../include/iDynTree/L2NormCost.h             |  4 ++
 .../OCSolvers/MultipleShootingSolver.h        |  2 +
 src/optimalcontrol/src/L2NormCost.cpp         | 44 +++++++++++++++++++
 .../src/MultipleShootingSolver.cpp            | 13 ++++--
 .../tests/MultipleShootingTest.cpp            |  4 +-
 5 files changed, 62 insertions(+), 5 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index 38017534b9a..c35b2c2b704 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -55,12 +55,16 @@ namespace iDynTree {
 
             bool setStateWeight(const MatrixDynSize& stateWeights);
 
+            bool setStateWeight(const VectorDynSize& stateWeights);
+
             bool setStateDesiredPoint(const VectorDynSize& desiredPoint);
 
             bool setStateDesiredTrajectory(std::shared_ptr<TimeVaryingVector> stateDesiredTrajectory);
 
             bool setControlWeight(const MatrixDynSize& controlWeights);
 
+            bool setControlWeight(const VectorDynSize& controlWeights);
+
             bool setControlDesiredPoint(const VectorDynSize& desiredPoint);
 
             bool setControlDesiredTrajectory(std::shared_ptr<TimeVaryingVector> controlDesiredTrajectory);
diff --git a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
index e44e8741673..2a85fb847e9 100644
--- a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
+++ b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
@@ -64,6 +64,8 @@ namespace iDynTree {
 
             bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations);
 
+            bool getPossibleTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations);
+
             virtual bool solve() override;
 
             bool getSolution(std::vector<VectorDynSize>& states, std::vector<VectorDynSize>& controls);
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 7409a932e7a..a240332b562 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -155,6 +155,20 @@ namespace iDynTree {
                 return true;
             }
 
+            bool setWeightMatrix(const VectorDynSize &weights)
+            {
+                if (weights.size() != this->selector->size()) {
+                    reportError("L2NormCost", "setWeightMatrix", "The weights matrix dimensions do not match those of the specified selector.");
+                    return false;
+                }
+
+                iDynTree::toEigen(this->weightMatrix) = iDynTree::toEigen(weights).asDiagonal();
+                toEigen(this->gradientSubMatrix) = 0.5 * toEigen(this->selector->asSelectorMatrix()).transpose() * (toEigen(this->weightMatrix) + toEigen(this->weightMatrix).transpose());
+                toEigen(this->hessianMatrix) = toEigen(this->gradientSubMatrix) * toEigen(this->selector->asSelectorMatrix());
+
+                return true;
+            }
+
             bool changeSelector(const MatrixDynSize& inputSelector) {
                 if ((inputSelector.rows() != this->selector->size()) || (inputSelector.cols() != this->selector->asSelectorMatrix().cols())) {
                     reportError("L2NormCost", "getObject", "The new selector dimensionsions do not match the old one.");
@@ -368,6 +382,21 @@ namespace iDynTree {
             return true;
         }
 
+        bool L2NormCost::setStateWeight(const VectorDynSize &stateWeights)
+        {
+            if (!(m_pimpl->stateCost.isValid())) {
+                reportError("L2NormCost", "setStateWeight", "The state cost portion has been deactivated, given the provided selectors.");
+                return false;
+            }
+
+            if (!(m_pimpl->stateCost.setWeightMatrix(stateWeights))) {
+                reportError("L2NormCost", "setStateWeight", "Error when specifying the state weights.");
+                return false;
+            }
+
+            return true;
+        }
+
         bool L2NormCost::setStateDesiredPoint(const VectorDynSize &desiredPoint)
         {
             if (!(m_pimpl->stateCost.isValid())) {
@@ -419,6 +448,21 @@ namespace iDynTree {
             return true;
         }
 
+        bool L2NormCost::setControlWeight(const VectorDynSize &controlWeights)
+        {
+            if (!(m_pimpl->stateCost.isValid())) {
+                reportError("L2NormCost", "setControlWeight", "The control cost portion has been deactivated, given the provided selectors.");
+                return false;
+            }
+
+            if (!(m_pimpl->controlCost.setWeightMatrix(controlWeights))) {
+                reportError("L2NormCost", "setControlWeight", "Error when specifying the control weights.");
+                return false;
+            }
+
+            return true;
+        }
+
         bool L2NormCost::setControlDesiredPoint(const VectorDynSize &desiredPoint)
         {
             if (!(m_pimpl->stateCost.isValid())) {
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index f4ca5b5f822..7cae317ea12 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -756,9 +756,11 @@ namespace iDynTree {
                 return true;
             }
 
-            bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations) {
+            bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations, bool printWarning) {
                 if (!(m_prepared)){
-                    reportWarning("MultipleShootingTranscription", "getTimings", "The method solve was not called yet. Computing new mesh points. These may be overwritten when calling the solve method.");
+                    if (printWarning) {
+                        reportWarning("MultipleShootingTranscription", "getTimings", "The method solve was not called yet. Computing new mesh points. These may be overwritten when calling the solve method.");
+                    }
 
                     if (!preliminaryChecks()) {
                         return false;
@@ -1653,7 +1655,12 @@ namespace iDynTree {
 
         bool MultipleShootingSolver::getTimings(std::vector<double> &stateEvaluations, std::vector<double> &controlEvaluations)
         {
-            return m_transcription->getTimings(stateEvaluations, controlEvaluations);
+            return m_transcription->getTimings(stateEvaluations, controlEvaluations, true);
+        }
+
+        bool MultipleShootingSolver::getPossibleTimings(std::vector<double> &stateEvaluations, std::vector<double> &controlEvaluations)
+        {
+            return m_transcription->getTimings(stateEvaluations, controlEvaluations, false);
         }
 
 
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 8b1e913d621..cb4795905fb 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -265,7 +265,7 @@ class OptimizerTest : public iDynTree::optimization::Optimizer {
 
     virtual ~OptimizerTest() override {}
 
-    virtual bool isAvailable() override{
+    virtual bool isAvailable() const override{
         return true;
     }
 
@@ -382,7 +382,7 @@ int main(){
     ASSERT_IS_TRUE(solver.setOptimizer(optimizer));
 
     std::vector<double> stateTimings, controlTimings;
-    ASSERT_IS_TRUE(solver.getTimings(stateTimings, controlTimings));
+    ASSERT_IS_TRUE(solver.getPossibleTimings(stateTimings, controlTimings));
     for (size_t i = 0; i < stateTimings.size(); ++i){
         ASSERT_IS_TRUE((stateTimings[i] > initTime) && (stateTimings[i] <= endTime));
         if (i > 0)

From 24281508c35e020b453ca6dcf5d7f4c7186489d3 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sat, 15 Sep 2018 11:18:09 +0200
Subject: [PATCH 048/194] Fixed bug in L2NormCost.

---
 src/optimalcontrol/src/L2NormCost.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index a240332b562..8679abb5b5e 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -435,7 +435,7 @@ namespace iDynTree {
                 return false;
             }
 
-            if (!(m_pimpl->stateCost.isValid())) {
+            if (!(m_pimpl->controlCost.isValid())) {
                 reportError("L2NormCost", "setControlWeight", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
@@ -450,7 +450,7 @@ namespace iDynTree {
 
         bool L2NormCost::setControlWeight(const VectorDynSize &controlWeights)
         {
-            if (!(m_pimpl->stateCost.isValid())) {
+            if (!(m_pimpl->controlCost.isValid())) {
                 reportError("L2NormCost", "setControlWeight", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
@@ -465,7 +465,7 @@ namespace iDynTree {
 
         bool L2NormCost::setControlDesiredPoint(const VectorDynSize &desiredPoint)
         {
-            if (!(m_pimpl->stateCost.isValid())) {
+            if (!(m_pimpl->controlCost.isValid())) {
                 reportError("L2NormCost", "setControlDesiredPoint", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }
@@ -481,7 +481,7 @@ namespace iDynTree {
 
         bool L2NormCost::setControlDesiredTrajectory(std::shared_ptr<TimeVaryingVector> controlDesiredTrajectory)
         {
-            if (!(m_pimpl->stateCost.isValid())) {
+            if (!(m_pimpl->controlCost.isValid())) {
                 reportError("L2NormCost", "setControlDesiredTrajectory", "The control cost portion has been deactivated, given the provided selectors.");
                 return false;
             }

From 4caf53c1889c0c51f0631da725b90cb0fa56a66e Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 16 Sep 2018 13:53:34 +0200
Subject: [PATCH 049/194] Fixed bug when the control period is equal to the
 minimum time.

An ignored mesh was not "ignore". Timings now are more "precise", i.e. before some numerical errors were accomulated.
---
 .../src/MultipleShootingSolver.cpp            | 62 +++++++++++++++----
 .../tests/MultipleShootingTest.cpp            | 25 +++++++-
 2 files changed, 74 insertions(+), 13 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 7cae317ea12..b85a711da85 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -317,8 +317,9 @@ namespace iDynTree {
                     newMeshPoint.time = time;
 
                     addMeshPoint(newMeshPoint);
-                    time += m_controlPeriod;
                     controlMeshes++;
+
+                    time = initTime + controlMeshes * m_controlPeriod;
                 }
 
                 std::vector<MeshPoint>::iterator lastControlMeshIterator = (m_meshPointsEnd)-1;
@@ -337,6 +338,7 @@ namespace iDynTree {
                     if ((lastControlMeshIterator->time + m_minStepSize) > endTime){ //if last control mesh point is too close to the end, remove it and place a state mesh point at the end
                         setIgnoredMesh(lastControlMeshIterator);
                         controlMeshes--;
+                        m_meshPointsEnd = lastControlMeshIterator;
                     }
                     addMeshPoint(newMeshPoint);  //add a mesh point at the end;
                 }
@@ -756,19 +758,51 @@ namespace iDynTree {
                 return true;
             }
 
-            bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations, bool printWarning) {
+            bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations) {
                 if (!(m_prepared)){
-                    if (printWarning) {
-                        reportWarning("MultipleShootingTranscription", "getTimings", "The method solve was not called yet. Computing new mesh points. These may be overwritten when calling the solve method.");
-                    }
+                    reportWarning("MultipleShootingTranscription", "getTimings", "The method solve was not called yet. Use the method getPossibleTimings instead.");
 
-                    if (!preliminaryChecks()) {
-                        return false;
-                    }
+                    return false;
+                }
 
-                    if (!setMeshPoints()){
-                        return false;
+                if (stateEvaluations.size() != (m_totalMeshes - 1)) {
+                    stateEvaluations.resize(m_totalMeshes - 1);
+                }
+
+                if (controlEvaluations.size() != m_controlMeshes) {
+                    controlEvaluations.resize(m_controlMeshes);
+                }
+
+                size_t stateIndex = 0, controlIndex = 0;
+
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                MeshPointOrigin ignored = MeshPointOrigin::Ignored();
+
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+
+                    assert(mesh->origin != ignored);
+
+                    if (mesh->origin != first){
+                        stateEvaluations[stateIndex] = mesh->time;
+                        stateIndex++;
                     }
+                    if (mesh->type == MeshPointType::Control){
+                        controlEvaluations[controlIndex] = mesh->time;
+                        controlIndex++;
+                    }
+                }
+
+                return true;
+            }
+
+            bool getPossibleTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations) {
+
+                if (!preliminaryChecks()) {
+                    return false;
+                }
+
+                if (!setMeshPoints()){
+                    return false;
                 }
 
                 if (stateEvaluations.size() != (m_totalMeshes - 1)) {
@@ -782,8 +816,12 @@ namespace iDynTree {
                 size_t stateIndex = 0, controlIndex = 0;
 
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                MeshPointOrigin ignored = MeshPointOrigin::Ignored();
 
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+
+                    assert(mesh->origin != ignored);
+
                     if (mesh->origin != first){
                         stateEvaluations[stateIndex] = mesh->time;
                         stateIndex++;
@@ -1655,12 +1693,12 @@ namespace iDynTree {
 
         bool MultipleShootingSolver::getTimings(std::vector<double> &stateEvaluations, std::vector<double> &controlEvaluations)
         {
-            return m_transcription->getTimings(stateEvaluations, controlEvaluations, true);
+            return m_transcription->getTimings(stateEvaluations, controlEvaluations);
         }
 
         bool MultipleShootingSolver::getPossibleTimings(std::vector<double> &stateEvaluations, std::vector<double> &controlEvaluations)
         {
-            return m_transcription->getTimings(stateEvaluations, controlEvaluations, false);
+            return m_transcription->getPossibleTimings(stateEvaluations, controlEvaluations);
         }
 
 
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index cb4795905fb..26cafc41675 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -386,7 +386,7 @@ int main(){
     for (size_t i = 0; i < stateTimings.size(); ++i){
         ASSERT_IS_TRUE((stateTimings[i] > initTime) && (stateTimings[i] <= endTime));
         if (i > 0)
-            ASSERT_IS_TRUE(((stateTimings[i] - stateTimings[i-1]) >= minStep) && ((stateTimings[i] - stateTimings[i-1]) <= maxStep));
+            ASSERT_IS_TRUE(((stateTimings[i] - stateTimings[i-1]) >= (0.999 * minStep)) && ((stateTimings[i] - stateTimings[i-1]) <= (1.001 * maxStep)));
     }
 
     for (size_t i = 0; i < controlTimings.size(); ++i){
@@ -397,6 +397,29 @@ int main(){
 
     ASSERT_IS_TRUE(solver.solve());
 
+    minStep = 0.003;
+    maxStep = 0.07;
+    controlPeriod = 0.003;
+    ASSERT_IS_TRUE(solver.setStepSizeBounds(minStep, maxStep));
+    ASSERT_IS_TRUE(solver.setControlPeriod(controlPeriod));
+
+
+    ASSERT_IS_TRUE(solver.getPossibleTimings(stateTimings, controlTimings));
+    for (size_t i = 0; i < stateTimings.size(); ++i){
+        ASSERT_IS_TRUE((stateTimings[i] > initTime) && (stateTimings[i] <= endTime));
+        if (i > 0)
+            ASSERT_IS_TRUE(((stateTimings[i] - stateTimings[i-1]) >= (0.999 * minStep)) && ((stateTimings[i] - stateTimings[i-1]) <= (1.001 * maxStep)));
+    }
+
+    for (size_t i = 0; i < controlTimings.size(); ++i){
+        ASSERT_IS_TRUE((controlTimings[i] >= initTime) && (controlTimings[i] <= endTime));
+        if (i > 0)
+            ASSERT_EQUAL_DOUBLE((controlTimings[i] - controlTimings[i-1]), controlPeriod);
+    }
+
+    ASSERT_IS_TRUE(solver.solve());
+
+
 
     return EXIT_SUCCESS;
 }

From c388dc316854e5c1e1b6d39bdb92066894f6db18 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 16 Sep 2018 13:59:17 +0200
Subject: [PATCH 050/194] Make MultipleShootingTest a bit faster.

---
 src/optimalcontrol/tests/MultipleShootingTest.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 26cafc41675..2e1558db1bd 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -363,7 +363,7 @@ int main(){
     iDynTree::VectorDynSize newBounds(1);
     newBounds(0) = 5.0;
     ASSERT_IS_TRUE(constraint2->setUpperBound(newBounds));
-    double initTime = 1.0, endTime = 5.0;
+    double initTime = 1.0, endTime = 2.0;
     double minStep = 0.003, maxStep = 0.07, controlPeriod = 0.011;
 
     ASSERT_IS_TRUE(problem->setTimeHorizon(initTime, endTime));

From b006878cd77b6146bc502fa5cdfd4bfd4e86176f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 16 Sep 2018 22:48:24 +0200
Subject: [PATCH 051/194] Moved guess setting to the optimization problem
 definition.

---
 .../include/iDynTree/OptimizationProblem.h       |  2 ++
 src/optimalcontrol/include/iDynTree/Optimizer.h  |  2 --
 .../include/iDynTree/Optimizers/IpoptInterface.h |  2 --
 .../include/iDynTree/Optimizers/OsqpInterface.h  |  2 --
 src/optimalcontrol/src/IpoptInterface.cpp        |  8 +-------
 src/optimalcontrol/src/OptimizationProblem.cpp   |  5 +++++
 src/optimalcontrol/src/Optimizer.cpp             |  6 ------
 src/optimalcontrol/src/OsqpInterface.cpp         | 16 +++++++---------
 .../tests/MultipleShootingTest.cpp               |  4 ----
 src/optimalcontrol/tests/OptimizerTest.cpp       |  7 ++++++-
 10 files changed, 21 insertions(+), 33 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
index b5f41302982..e1a8ab01665 100644
--- a/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimizationProblem.h
@@ -109,6 +109,8 @@ namespace iDynTree {
 
             virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns); //costs and constraints together
 
+            virtual bool getGuess(VectorDynSize &guess);
+
             virtual bool setVariables(const VectorDynSize& variables);
 
             virtual bool evaluateCostFunction(double& costValue);
diff --git a/src/optimalcontrol/include/iDynTree/Optimizer.h b/src/optimalcontrol/include/iDynTree/Optimizer.h
index 6f9d8817096..c36e8327cea 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizer.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizer.h
@@ -48,8 +48,6 @@ namespace iDynTree {
 
             virtual const std::weak_ptr<OptimizationProblem> problem() const;
 
-            virtual bool setInitialGuess(VectorDynSize &initialGuess);
-
             virtual bool solve() = 0; //warm start capabilities should be implemented in the solver specific interface
 
             virtual bool getPrimalVariables(VectorDynSize &primalVariables);
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
index 07455c279b9..5aadb8a4a81 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
@@ -49,8 +49,6 @@ namespace iDynTree {
 
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
 
-            virtual bool setInitialGuess(VectorDynSize &initialGuess) override;
-
             virtual bool solve() override;
 
             virtual bool getPrimalVariables(VectorDynSize &primalVariables) override;
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
index d05c08df77f..ad9f074877f 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/OsqpInterface.h
@@ -74,8 +74,6 @@ namespace iDynTree {
 
             virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
 
-            virtual bool setInitialGuess(VectorDynSize &initialGuess) override;
-
             virtual bool solve() override;
 
             virtual bool getPrimalVariables(VectorDynSize &primalVariables) override;
diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index 00b157901dd..a19a4e815e5 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -190,6 +190,7 @@ namespace iDynTree {
 
                 if(init_x){
                     Eigen::Map<Eigen::VectorXd> x_map(x, n);
+                    initialGuessSet = problem->getGuess(initialGuess);
                     if (initialGuessSet) {
                         if (initialGuess.size() == static_cast<unsigned int>(n)){
                             x_map = iDynTree::toEigen(initialGuess);
@@ -511,13 +512,6 @@ namespace iDynTree {
             return true;
         }
 
-        bool IpoptInterface::setInitialGuess(VectorDynSize &initialGuess)
-        {
-            m_pimpl->nlpPointer->initialGuessSet = true;
-            m_pimpl->nlpPointer->initialGuess = initialGuess;
-            return true;
-        }
-
         bool IpoptInterface::solve()
         {
             if (!m_problem) {
diff --git a/src/optimalcontrol/src/OptimizationProblem.cpp b/src/optimalcontrol/src/OptimizationProblem.cpp
index c956db02feb..15220683a25 100644
--- a/src/optimalcontrol/src/OptimizationProblem.cpp
+++ b/src/optimalcontrol/src/OptimizationProblem.cpp
@@ -72,6 +72,11 @@ namespace iDynTree {
             return false;
         }
 
+        bool OptimizationProblem::getGuess(VectorDynSize &guess)
+        {
+            return false;
+        }
+
         bool OptimizationProblem::setVariables(const VectorDynSize &variables)
         {
             reportError("OptimizationProblem", "setVariables", "Method not implemented.");
diff --git a/src/optimalcontrol/src/Optimizer.cpp b/src/optimalcontrol/src/Optimizer.cpp
index f63d944586e..1a09b55d00f 100644
--- a/src/optimalcontrol/src/Optimizer.cpp
+++ b/src/optimalcontrol/src/Optimizer.cpp
@@ -42,12 +42,6 @@ namespace iDynTree {
             return m_problem;
         }
 
-        bool Optimizer::setInitialGuess(VectorDynSize &initialGuess)
-        {
-            reportError("Optimizer", "setInitialGuess", "Method not implemented.");
-            return false;
-        }
-
         bool Optimizer::getPrimalVariables(VectorDynSize &primalVariables)
         {
             reportError("Optimizer", "getPrimalVariables", "Method not implemented.");
diff --git a/src/optimalcontrol/src/OsqpInterface.cpp b/src/optimalcontrol/src/OsqpInterface.cpp
index b5688e562b3..3bd45fa7ce5 100644
--- a/src/optimalcontrol/src/OsqpInterface.cpp
+++ b/src/optimalcontrol/src/OsqpInterface.cpp
@@ -243,7 +243,7 @@ namespace iDynTree {
             std::shared_ptr<std::vector<size_t>> constraintNNZRows, constraintNNZCols;
             std::shared_ptr<std::vector<size_t>> hessianNNZRows, hessianNNZCols;
             VectorDynSize variablesBuffer, solutionBuffer, constraintsBuffer;
-            VectorDynSize costGradient;
+            VectorDynSize costGradient, iDynTreeInitialGuess;
             std::shared_ptr<MatrixDynSize> costHessian;
             std::shared_ptr<MatrixDynSize> constraintJacobian;
             unsigned int nv, nc, previous_nv, previous_nc;
@@ -452,14 +452,6 @@ namespace iDynTree {
             return true;
         }
 
-        bool OsqpInterface::setInitialGuess(VectorDynSize &initialGuess)
-        {
-            m_pimpl->initialGuessSet = true;
-            m_pimpl->eigenInitialGuess.resize(initialGuess.size());
-            m_pimpl->eigenInitialGuess = toEigen(initialGuess);
-            return true;
-        }
-
         bool OsqpInterface::solve()
         {
             if (!m_problem) {
@@ -653,6 +645,9 @@ namespace iDynTree {
                     return false;
                 }
 
+                m_pimpl->initialGuessSet = m_problem->getGuess(m_pimpl->iDynTreeInitialGuess);
+                m_pimpl->eigenInitialGuess.resize(m_pimpl->iDynTreeInitialGuess.size());
+                m_pimpl->eigenInitialGuess = toEigen(m_pimpl->iDynTreeInitialGuess);
                 if (m_pimpl->initialGuessSet) {
                     if (m_pimpl->eigenInitialGuess.size() != m_pimpl->nv) {
                         reportError("OsqpInterface", "solve", "The specified intial guess has dimension different from the number of variables.");
@@ -719,6 +714,9 @@ namespace iDynTree {
                     return false;
                 }
 
+                m_pimpl->initialGuessSet = m_problem->getGuess(m_pimpl->iDynTreeInitialGuess);
+                m_pimpl->eigenInitialGuess.resize(m_pimpl->iDynTreeInitialGuess.size());
+                m_pimpl->eigenInitialGuess = toEigen(m_pimpl->iDynTreeInitialGuess);
                 if (m_pimpl->initialGuessSet) {
                     if (m_pimpl->eigenInitialGuess.size() != m_pimpl->nv) {
                         reportError("OsqpInterface", "solve", "The specified intial guess has dimension different from the number of variables.");
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 2e1558db1bd..8d08d602e8b 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -269,10 +269,6 @@ class OptimizerTest : public iDynTree::optimization::Optimizer {
         return true;
     }
 
-    virtual bool setInitialGuess(iDynTree::VectorDynSize &initialGuess) override{
-        return true;
-    }
-
     virtual bool solve() override {
         iDynTree::VectorDynSize dummyVariables, dummy1, dummy2;
         iDynTree::MatrixDynSize dummyMatrix, jacobian;
diff --git a/src/optimalcontrol/tests/OptimizerTest.cpp b/src/optimalcontrol/tests/OptimizerTest.cpp
index db6a55d9780..e34b02926fa 100644
--- a/src/optimalcontrol/tests/OptimizerTest.cpp
+++ b/src/optimalcontrol/tests/OptimizerTest.cpp
@@ -58,6 +58,12 @@ class TestProblem : public iDynTree::optimization::OptimizationProblem {
         return 2;
     }
 
+    virtual bool getGuess(iDynTree::VectorDynSize &guess) override {
+        guess.resize(2);
+        guess.zero();
+        return true;
+    }
+
     virtual bool getConstraintsBounds(iDynTree::VectorDynSize& constraintsLowerBounds, iDynTree::VectorDynSize& constraintsUpperBounds) override {
         constraintsLowerBounds.resize(2);
         constraintsUpperBounds.resize(2);
@@ -200,7 +206,6 @@ int main(){
 
     ASSERT_IS_TRUE(ipoptSolver.setIpoptOption("nlp_lower_bound_inf", -1.0e20));
     ASSERT_IS_TRUE(ipoptSolver.setIpoptOption("print_level", 0));
-    ASSERT_IS_TRUE(ipoptSolver.setInitialGuess(guess));
 
     ASSERT_IS_TRUE(problem->setMinusInfinity(ipoptSolver.minusInfinity()));
     ASSERT_IS_TRUE(problem->setPlusInfinity(ipoptSolver.plusInfinity()));

From 50a2c90bb7e02bf7746f87d4659125cc7cb50c50 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 18 Sep 2018 08:41:55 +0200
Subject: [PATCH 052/194] Added destructor in TimeInvariantObject.

---
 src/optimalcontrol/include/iDynTree/TimeVaryingObject.h | 2 ++
 src/optimalcontrol/src/TimeVaryingObject.cpp            | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
index b0cc1f761cd..9535a8d2b8b 100644
--- a/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
+++ b/src/optimalcontrol/include/iDynTree/TimeVaryingObject.h
@@ -66,6 +66,8 @@ namespace iDynTree {
 
             TimeInvariantObject(const Object& timeInvariantObject);
 
+            ~TimeInvariantObject() final;
+
             Object& get();
 
             virtual const Object& get(double time, bool &isValid) final;
diff --git a/src/optimalcontrol/src/TimeVaryingObject.cpp b/src/optimalcontrol/src/TimeVaryingObject.cpp
index 76a729c95db..aef1de59849 100644
--- a/src/optimalcontrol/src/TimeVaryingObject.cpp
+++ b/src/optimalcontrol/src/TimeVaryingObject.cpp
@@ -28,6 +28,10 @@ namespace iDynTree {
             : m_invariantObject(timeInvariantObject)
         { }
 
+        template<typename Object>
+        TimeInvariantObject<Object>::~TimeInvariantObject()
+        { }
+
         template<typename Object>
         Object &TimeInvariantObject<Object>::get()
         {

From 6795d744545dced7357dcbb904ceccccb7c857f4 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 18 Sep 2018 08:44:02 +0200
Subject: [PATCH 053/194] Added possibility to set guesses in
 MultipleShootingSolver.

---
 .../OCSolvers/MultipleShootingSolver.h        |   4 +
 .../src/MultipleShootingSolver.cpp            | 111 +++++++++++++++++-
 2 files changed, 114 insertions(+), 1 deletion(-)

diff --git a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
index 2a85fb847e9..01728130666 100644
--- a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
+++ b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
@@ -18,6 +18,7 @@
 #define IDYNTREE_OPTIMALCONTROL_MULTIPLESHOOTINGSOLVER_H
 
 #include <iDynTree/OptimalControlSolver.h>
+#include <iDynTree/TimeVaryingObject.h>
 #include <iDynTree/Optimizer.h>
 
 #include <vector>
@@ -62,6 +63,9 @@ namespace iDynTree {
 
             bool setInitialState(const VectorDynSize &initialState);
 
+            bool setGuesses(std::shared_ptr<optimalcontrol::TimeVaryingVector> stateGuesses,
+                            std::shared_ptr<optimalcontrol::TimeVaryingVector> controlGuesses);
+
             bool getTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations);
 
             bool getPossibleTimings(std::vector<double>& stateEvaluations, std::vector<double>& controlEvaluations);
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index b85a711da85..d98a2299bd4 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -100,12 +100,13 @@ namespace iDynTree {
             size_t m_jacobianNonZeros, m_hessianNonZeros;
             double m_plusInfinity, m_minusInfinity;
             VectorDynSize m_constraintsLowerBound, m_constraintsUpperBound;
-            VectorDynSize m_constraintsBuffer, m_stateBuffer, m_controlBuffer, m_variablesBuffer, m_costStateGradientBuffer, m_costControlGradientBuffer;
+            VectorDynSize m_constraintsBuffer, m_stateBuffer, m_controlBuffer, m_variablesBuffer, m_guessBuffer, m_costStateGradientBuffer, m_costControlGradientBuffer;
             MatrixDynSize m_costHessianStateBuffer, m_costHessianControlBuffer, m_costHessianStateControlBuffer, m_costHessianControlStateBuffer;
             std::vector<VectorDynSize> m_collocationStateBuffer, m_collocationControlBuffer;
             std::vector<MatrixDynSize> m_collocationStateJacBuffer, m_collocationControlJacBuffer;
             MatrixDynSize m_constraintsStateJacBuffer, m_constraintsControlJacBuffer;
             VectorDynSize m_solution;
+            std::shared_ptr<TimeVaryingVector> m_stateGuesses, m_controlGuesses;
             bool m_solved;
 
             friend class MultipleShootingSolver;
@@ -240,6 +241,8 @@ namespace iDynTree {
                     m_variablesBuffer.resize(static_cast<unsigned int>(m_numberOfVariables));
                 }
 
+                m_guessBuffer.resize(static_cast<unsigned int>(m_numberOfVariables));
+
                 if (m_constraintsBuffer.size() != m_constraintsPerInstant) {
                     m_constraintsBuffer.resize(static_cast<unsigned int>(m_constraintsPerInstant));
                 }
@@ -896,6 +899,8 @@ namespace iDynTree {
             , m_numberOfConstraints(0)
             , m_plusInfinity(1e19)
             , m_minusInfinity(-1e19)
+            , m_stateGuesses(nullptr)
+            , m_controlGuesses(nullptr)
             , m_solved(false)
             { }
 
@@ -916,6 +921,8 @@ namespace iDynTree {
             , m_numberOfConstraints(0)
             , m_plusInfinity(1e19)
             , m_minusInfinity(-1e19)
+            , m_stateGuesses(nullptr)
+            , m_controlGuesses(nullptr)
             , m_solved(false)
             { }
 
@@ -1301,6 +1308,84 @@ namespace iDynTree {
                 return true;
             }
 
+            bool getGuess(VectorDynSize &guess) override {
+                if (!m_stateGuesses || !m_controlGuesses) { //no guess is available (this is not an error)
+                    return false;
+                }
+
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "getGuess", "First you need to call the prepare method");
+                    return false;
+                }
+
+                iDynTreeEigenVector guessMap = toEigen(m_guessBuffer);
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
+
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                bool isValid = false;
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+                    if (mesh->origin == first){
+                        guessMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) =
+                                toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+
+                        const iDynTree::VectorDynSize &controlGuess = m_controlGuesses->get(mesh->time, isValid);
+
+                        if (!isValid || (controlGuess.size() != nu)) {
+                            std::ostringstream errorMsg;
+                            errorMsg << "Unable to get a valid control guess at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "getGuess", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        guessMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = toEigen(controlGuess);
+
+                        guessMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) =
+                                toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
+
+                    } else if (mesh->type == MeshPointType::Control) {
+
+                        const iDynTree::VectorDynSize &controlGuess = m_controlGuesses->get(mesh->time, isValid);
+
+                        if (!isValid || (controlGuess.size() != nu)) {
+                            std::ostringstream errorMsg;
+                            errorMsg << "Unable to get a valid control guess at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "getGuess", errorMsg.str().c_str());
+                            return false;
+                        }
+
+
+                        const iDynTree::VectorDynSize &stateGuess = m_stateGuesses->get(mesh->time, isValid);
+
+                        if (!isValid || (stateGuess.size() != nx)) {
+                            std::ostringstream errorMsg;
+                            errorMsg << "Unable to get a valid state guess at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "getGuess", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        guessMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = toEigen(controlGuess);
+                        guessMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = toEigen(stateGuess);
+                    } else if (mesh->type == MeshPointType::State) {
+
+                        const iDynTree::VectorDynSize &stateGuess = m_stateGuesses->get(mesh->time, isValid);
+
+                        if (!isValid || (stateGuess.size() != nx)) {
+                            std::ostringstream errorMsg;
+                            errorMsg << "Unable to get a valid state guess at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "getGuess", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        guessMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = toEigen(stateGuess);
+                    }
+                }
+
+                guess = m_guessBuffer;
+
+                return true;
+            }
+
             virtual bool setVariables(const VectorDynSize& variables) override {
                 if (variables.size() != m_variablesBuffer.size()){
                     reportError("MultipleShootingTranscription", "setVariables", "The input variables have a size different from the expected one.");
@@ -1691,6 +1776,26 @@ namespace iDynTree {
             return m_transcription->setInitialState(initialState);
         }
 
+        bool MultipleShootingSolver::setGuesses(std::shared_ptr<TimeVaryingVector> stateGuesses,
+                                                std::shared_ptr<TimeVaryingVector> controlGuesses)
+        {
+            if (!stateGuesses) {
+                reportError("MultipleShootingSolver", "setGuesses", "Empty state guess pointer.");
+                return false;
+            }
+
+            if (!controlGuesses) {
+                reportError("MultipleShootingSolver", "setGuesses", "Empty control guess pointer.");
+                return false;
+            }
+
+            m_transcription->m_stateGuesses = stateGuesses;
+            m_transcription->m_controlGuesses = controlGuesses;
+
+            return true;
+
+        }
+
         bool MultipleShootingSolver::getTimings(std::vector<double> &stateEvaluations, std::vector<double> &controlEvaluations)
         {
             return m_transcription->getTimings(stateEvaluations, controlEvaluations);
@@ -1718,6 +1823,10 @@ namespace iDynTree {
                 return false;
             }
             m_transcription->m_solved = true;
+
+            m_transcription->m_stateGuesses = nullptr; //Guesses are used only once
+            m_transcription->m_controlGuesses = nullptr;
+
             return true;
         }
 

From 3d25a055a415bf393c9338ca94f7cec15266e41c Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 18 Sep 2018 08:44:26 +0200
Subject: [PATCH 054/194] Added option to disable compilation of optimalcontrol
 part.

---
 cmake/iDynTreeOptions.cmake | 5 +++++
 src/CMakeLists.txt          | 4 +++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/cmake/iDynTreeOptions.cmake b/cmake/iDynTreeOptions.cmake
index ec8420ad238..e051f9e9adb 100644
--- a/cmake/iDynTreeOptions.cmake
+++ b/cmake/iDynTreeOptions.cmake
@@ -47,6 +47,11 @@ endif()
 # Turn on compilation of geometrical relations semantics check.
 option(IDYNTREE_USES_SEMANTICS "Compile iDynTree semantics check" FALSE)
 
+
+#########################################################################
+# Turn off compilation of optimal control part.
+option(IDYNTREE_COMPILES_OPTIMALCONTROL "Compile iDynTree optimal control part." TRUE)
+
 #########################################################################
 # Deal with RPATH
 option(IDYNTREE_ENABLE_RPATH "Enable RPATH for the library" TRUE)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c59ead73e73..063687913d3 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -25,7 +25,9 @@ if (IDYNTREE_USES_IPOPT)
   add_subdirectory(inverse-kinematics)
 endif()
 
-add_subdirectory(optimalcontrol)
+if (IDYNTREE_COMPILES_OPTIMALCONTROL)
+  add_subdirectory(optimalcontrol)
+endif()
 
 if(IDYNTREE_USES_YARP)
     add_subdirectory(yarp)

From 6b651a1db44104d8cee8d2eaeaabc8d627b3d207 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 21 Sep 2018 18:41:51 +0200
Subject: [PATCH 055/194] Fixed bug when using explicit integrators.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index d98a2299bd4..620e20bc432 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -330,6 +330,7 @@ namespace iDynTree {
                 if ((lastControlMeshIterator->origin == MeshPointOrigin::LastPoint()) && (m_integrator->info().isExplicit())) {//the last control input would have no effect
                     controlMeshes--;
                     setIgnoredMesh(lastControlMeshIterator);
+                    m_meshPointsEnd = lastControlMeshIterator;
                     --lastControlMeshIterator;
                 }
 

From 4da18942a5899b9923700dd0c20bca16e035af89 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 21 Sep 2018 20:27:19 +0200
Subject: [PATCH 056/194] Removed leftovers of dummy implementations of Ipopt
 and OsqpEigen interfaces.

---
 src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp | 6 ------
 src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp  | 6 ------
 2 files changed, 12 deletions(-)

diff --git a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
index 9c9ddb3d709..c5f04ada2ca 100644
--- a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
@@ -40,12 +40,6 @@ bool IpoptInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
     return false;
 }
 
-bool IpoptInterface::setInitialGuess(VectorDynSize &initialGuess)
-{
-    reportError("IpoptInterface", "setInitialGuess", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
-    return false;
-}
-
 bool IpoptInterface::solve()
 {
     reportError("IpoptInterface", "solve", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
diff --git a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
index 126bea7ae22..36888d12a05 100644
--- a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
@@ -37,12 +37,6 @@ bool OsqpInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
     return false;
 }
 
-bool OsqpInterface::setInitialGuess(VectorDynSize &initialGuess)
-{
-    reportError("OsqpInterface", "setInitialGuess", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");
-    return false;
-}
-
 bool OsqpInterface::solve()
 {
     reportError("OsqpInterface", "solve", "OsqpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_OSQPEIGEN set to ON?");

From 657b81e4989b81175b00aaebde30edc5fd164e0b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 23 Sep 2018 20:37:36 +0200
Subject: [PATCH 057/194] Fixed bug when last merge point is a control mesh
 point.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 620e20bc432..904a34e0414 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -579,6 +579,9 @@ namespace iDynTree {
                     newTotalMeshes++;
                     ++mesh;
                 }
+                if (mesh->type == MeshPointType::Control){
+                    newControlMeshes++;
+                }
                 newTotalMeshes++; //the last mesh
                 m_meshPointsEnd = m_meshPoints.begin() + static_cast<long>(newTotalMeshes);
                 assert((m_meshPointsEnd - 1)->origin == last);

From d75723cda0ce487377f75eab34a87041d08562eb Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 15 Oct 2018 14:29:22 +0200
Subject: [PATCH 058/194] Moved release notes to v0_12.md

---
 doc/releases/v0_12.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 5447674a41e..0f4dffbfc7e 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -20,3 +20,9 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 #### `inverse-kinematics`
 * Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).
 
+#### `optimalcontrol`
+* Added objects to deal with linear optimal control problems.
+* Added ``OSQP`` interface via ``osqp-eigen``.
+* Fixed bugs in ``MultipleShooting`` solver.
+* Added few lines of documentation.
+

From 851b5d6b213f2c06795ff67e4475ec2c9e0e8a49 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 15 Oct 2018 18:16:07 +0200
Subject: [PATCH 059/194] Using iDynTree namespace in "notImplemented"
 interfaces.

---
 src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp | 1 +
 src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp  | 1 +
 2 files changed, 2 insertions(+)

diff --git a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
index c5f04ada2ca..aebe7431aff 100644
--- a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
@@ -18,6 +18,7 @@
 #include <iDynTree/Core/Utils.h>
 
 using namespace iDynTree::optimization;
+using namespace iDynTree;
 
 IpoptInterface::IpoptInterface()
 {
diff --git a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
index 36888d12a05..abc3c6a5a05 100644
--- a/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/OsqpInterfaceNotImplemented.cpp
@@ -19,6 +19,7 @@
 
 
 using namespace iDynTree::optimization;
+using namespace iDynTree;
 
 OsqpInterface::OsqpInterface()
 { }

From 7049fa7c6601bf86b2f5e0b0d251f0f2eed70dab Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 16 Oct 2018 11:06:18 +0200
Subject: [PATCH 060/194] Add example on the use of the
 iDynTree.InverseKinematics class

---
 README.md                                     |   1 +
 examples/InverseKinematics/CMakeLists.txt     |  31 ++++
 examples/InverseKinematics/README.md          |  31 ++++
 .../iDynTreeExampleInverseKinematics.cpp      | 174 ++++++++++++++++++
 4 files changed, 237 insertions(+)
 create mode 100644 examples/InverseKinematics/CMakeLists.txt
 create mode 100644 examples/InverseKinematics/README.md
 create mode 100644 examples/InverseKinematics/iDynTreeExampleInverseKinematics.cpp

diff --git a/README.md b/README.md
index 2175b7dfe87..ac816d2e8b2 100644
--- a/README.md
+++ b/README.md
@@ -168,6 +168,7 @@ For more info on how to modify the matlab bindings, see https://github.com/robot
 |:------:|:---:|:------:|:------:|
 | Use of the [ExtWrenchesAndJointTorquesEstimator class](http://robotology.gitlab.io/docs/idyntree/master/classiDynTree_1_1ExtWrenchesAndJointTorquesEstimator.html) for computing offset for FT sensors | NA | [examples/matlab/SixAxisFTOffsetEstimation/SixAxisFTOffsetEstimation.m](examples/matlab/SixAxisFTOffsetEstimation/SixAxisFTOffsetEstimation.m) | NA |
 | How to get the axis of a revolute joint expressed in a arbitary frame using the [KinDynComputations class](http://robotology.gitlab.io/docs/idyntree/master/classiDynTree_1_1KinDynComputations.html) | NA | [ examples/matlab/GetJointAxesInWorldFrame.m](examples/matlab/GetJointAxesInWorldFrame.m) | NA |
+| How to use the [InverseKinematics class](http://robotology.gitlab.io/docs/idyntree/master/classiDynTree_1_1InverseKinematics.html) for the IK of an industrial fixed-base manipulator. | [examples/InverseKinematics/README.md](examples/InverseKinematics/README.md) | NA | NA |
 
 Are you interested in a tutorial on a specific feature or algorithm? Just [request it on an enhancement issue](https://github.com/robotology/idyntree/issues/new).
 
diff --git a/examples/InverseKinematics/CMakeLists.txt b/examples/InverseKinematics/CMakeLists.txt
new file mode 100644
index 00000000000..bfb3da1305e
--- /dev/null
+++ b/examples/InverseKinematics/CMakeLists.txt
@@ -0,0 +1,31 @@
+# Copyright: 2018 Fondazione Istituto Italiano di Tecnologia
+# Author: Silvio Traversaro
+# CopyPolicy: Released under the terms of the MIT License
+#
+
+cmake_minimum_required(VERSION 3.5)
+
+project(iDynTreeExampleInverseKinematics)
+
+find_package(iDynTree REQUIRED)
+
+source_group("Source Files" FILES iDynTreeExampleInverseKinematics.cpp)
+
+add_executable(${PROJECT_NAME} iDynTreeExampleInverseKinematics.cpp)
+target_link_libraries(${PROJECT_NAME} ${iDynTree_LIBRARIES})
+
+# Use M_PI also on Windows
+add_definitions(-D_USE_MATH_DEFINES)
+
+# To avoid vendoring the URDF files used for the example, let's download it on the fly
+if(NOT EXISTS ${CMAKE_CURRENT_BINARY_DIR}/kr16_2-buffer.urdf)
+  file(DOWNLOAD https://raw.githubusercontent.com/ros-industrial/kuka_experimental/indigo-devel/kuka_kr16_support/urdf/kr16_2.urdf ${CMAKE_CURRENT_BINARY_DIR}/kr16_2-buffer.urdf)
+endif()
+
+# For multiple config generators, the binary is not in the CMAKE_CURRENT_BINARY_DIR
+# let's make sure that we copy the file in the executable folder, using the TARGET_FILE_DIR
+# expressor generator ( https://cmake.org/cmake/help/latest/manual/cmake-generator-expressions.7.html )
+file(GENERATE OUTPUT $<TARGET_FILE_DIR:${PROJECT_NAME}>/kr16_2.urdf
+     INPUT ${CMAKE_CURRENT_BINARY_DIR}/kr16_2-buffer.urdf)
+
+
diff --git a/examples/InverseKinematics/README.md b/examples/InverseKinematics/README.md
new file mode 100644
index 00000000000..7e837fedf8d
--- /dev/null
+++ b/examples/InverseKinematics/README.md
@@ -0,0 +1,31 @@
+# iDynTree InverseKinematics Example
+
+This is an example of use of the iDynTree InverseKinematics
+
+## Compilation
+
+Make sure that you installed iDynTree with the `IDYNTREE_USES_IPOPT` CMake option selected, and your iDynTree installation
+can be found by CMake's [`find_package`](https://cmake.org/cmake/help/latest/command/find_package.html), either because it is installed in a system
+location or because its installation prefix is present in [`CMAKE_PREFIX_PATH`](https://cmake.org/cmake/help/latest/variable/CMAKE_PREFIX_PATH.html).
+
+After that, the example can be compiled as any CMake project, for example on *nix systems:
+~~~bash
+mkdir build
+cd build
+cmake ..
+make
+~~~
+
+For how to compile using IDEs, see https://cgold.readthedocs.io/en/latest/first-step.html .
+
+
+No installation target is provided, as the example is meant to be run from the build directory.
+
+## Run the example
+To run the example, go to the `build` directory and execute the `iDynTreeExampleInverseKinematics` executable.
+The executable assumes that a file `kr16_2.urdf` is available in the same directory where the executable as launched,
+so you need to launch the executable in its directory. The file `kr16_2.urdf` is downloaded by CMake during configuration.
+
+
+For a detailed overview of the example, check its source code.
+
diff --git a/examples/InverseKinematics/iDynTreeExampleInverseKinematics.cpp b/examples/InverseKinematics/iDynTreeExampleInverseKinematics.cpp
new file mode 100644
index 00000000000..1e4dd3120fc
--- /dev/null
+++ b/examples/InverseKinematics/iDynTreeExampleInverseKinematics.cpp
@@ -0,0 +1,174 @@
+/**
+* @ingroup idyntree_tutorials
+*
+* A tutorial on how to use the InverseKinematics class in iDynTree
+*
+* \author Silvio Traversaro
+*
+* CopyPolicy: Released under the terms of LGPL 2.0+ or later
+*/
+
+// C headers
+#include <cmath>
+#include <cstdlib>
+
+// C++ headers
+#include <string>
+#include <vector>
+
+// iDynTree headers
+#include <iDynTree/KinDynComputations.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/InverseKinematics.h>
+
+int main(int argc, char *argv[])
+{
+    // Assume that the model is found in the current working directory
+    // In production code, the model is tipical found using some strategy
+    std::string modelFile = "kr16_2.urdf";
+
+    // To make sure that the serialization of the joint is exactly the one that we need,
+    // it is a good practice to explicitly specify the desired joint/degrees of freedom
+    // serialization. Note that in production code this can be loaded from some parameter
+    std::vector<std::string> consideredJoints
+        = {"joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6"};
+
+
+    // Helper class to load the model from an external format
+    // Note: the loadReducedModelFromFile method permits to specify also a subset of the
+    // joint of the robot, hence the name "reduced". As we are specifying all the joints
+    // of the robot (we are just specifying them to make sure that the joint serialization
+    // is the one that we desire) we are actually loading a "full" model
+    iDynTree::ModelLoader mdlLoader;
+    bool ok = mdlLoader.loadReducedModelFromFile(modelFile, consideredJoints);
+
+    if (!ok) {
+        std::cerr << "iDynTreeExampleInverseKinematics: impossible to load the following model in a KinDynComputations class:" << std::endl
+                  << mdlLoader.model().toString() << std::endl;
+    }
+
+    // Create iDynTree::KinDynComputations, that can be used to compute the Forward Kinematics of robot
+    // We compute the Forward kinematics position initially to get a reasonable starting point for a cartesian position,
+    // and then to verify that the obtained Inverse Kinematics solution is actually closed to the desired one
+    iDynTree::KinDynComputations kinDynMeas;
+    ok = kinDynMeas.loadRobotModel(mdlLoader.model());
+
+    if (!ok) {
+        std::cerr << "iDynTreeExampleInverseKinematics: impossible to load model from " << modelFile << std::endl;
+        return EXIT_FAILURE;
+    }
+
+    // As different frames are available on the robot, we need to explicitly specify the frames used as base
+    // and as end-effector frames
+    std::string baseFrame = "base_link";
+    std::string endEffectorFrame = "tool0";
+
+    // All units in iDynTree are the one of the International System of Units (SI)
+    // Always pay attention to units, because other robotics software systems (such as YARP or KUKA)
+    // actually use degrees for representing angles
+    iDynTree::VectorDynSize jointPosInitialInRadians(mdlLoader.model().getNrOfPosCoords());
+    // Let's start from the classical KUKA KR** home position
+    double deg2rad = M_PI/180.0;
+    jointPosInitialInRadians(0) =   0.0*deg2rad;
+    jointPosInitialInRadians(1) = -90.0*deg2rad;
+    jointPosInitialInRadians(2) =  90.0*deg2rad;
+    jointPosInitialInRadians(3) =   0.0*deg2rad;
+    jointPosInitialInRadians(4) =  90.0*deg2rad;
+    jointPosInitialInRadians(5) =   0.0*deg2rad;
+
+    // The iDynTree::KinDynComputations actually supports also setting the base position
+    // and the base and joint velocity, but in this case we just use it for computing
+    // the forward kinematics between two frames in the model, so we just need to
+    kinDynMeas.setJointPos(jointPosInitialInRadians);
+
+    // Let's read the initial cartesian position
+    iDynTree::Transform base_H_ee_initial = kinDynMeas.getRelativeTransform(baseFrame, endEffectorFrame);
+
+    // Print the initial joint and cartesian position
+    std::cerr << "Initial cartesian linear position: " << std::endl << "\t" << base_H_ee_initial.getPosition().toString() << std::endl;
+    std::cerr << "Initial joint position (radians): " << std::endl << "\t" << jointPosInitialInRadians.toString() << std::endl;
+
+    // Let's define a new desired cartesian position, by decreasing the z-position of initial cartesian pose by 10 centimeters (0.1 meters):
+    // We keep the same rotation
+    iDynTree::Rotation initial_rot_pose = base_H_ee_initial.getRotation();
+    // But we change the liner position
+    iDynTree::Position initial_linear_pose = base_H_ee_initial.getPosition();
+    initial_linear_pose(2) = initial_linear_pose(2) - 0.1;
+    iDynTree::Transform base_H_ee_desired = iDynTree::Transform(initial_rot_pose, initial_linear_pose);
+
+
+    // Let's print the desired position
+    std::cerr << "Desired cartesian linear position: " << std::endl << "\t" << base_H_ee_desired.getPosition().toString() << std::endl;
+
+    // Create a InverseKinematics class using the loaded model
+    iDynTree::InverseKinematics ik;
+    ok = ik.setModel(mdlLoader.model());
+
+    if (!ok) {
+        std::cerr << "iDynTreeExampleInverseKinematics: impossible to load the following model in a InverseKinematics class:" << std::endl
+                  << mdlLoader.model().toString() << std::endl;
+        return EXIT_FAILURE;
+    }
+
+    // Setup Inverse Kinematics problem
+
+    // Set IK tolerances
+    ik.setCostTolerance(0.0001);
+    ik.setConstraintsTolerance(0.00001);
+
+    // Always consider targets as costs, as opposed to a
+    ik.setDefaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone);
+
+    // Use roll pitch yaw parametrization for the rotational part of the inverse kinematics
+    ik.setRotationParametrization(iDynTree::InverseKinematicsRotationParametrizationRollPitchYaw);
+
+    // Note: the InverseKinematics class actually implements a floating base inverse kinematics,
+    // meaning that both the joint position and the robot base are optimized to reach the desired cartesian position
+    // As in this example we are considering instead the fixed-base case, we impose that the desired position of the base
+    // is the identity
+    iDynTree::Transform world_H_base = iDynTree::Transform::Identity();
+    ok = ik.setFloatingBaseOnFrameNamed(baseFrame);
+    if (!ok) {
+        std::cerr << "iDynTreeExampleInverseKinematics: impossible to set floating base on " << baseFrame << std::endl;
+        return EXIT_FAILURE;
+    }
+    ik.addFrameConstraint(baseFrame, world_H_base);
+
+    // Add the desired task: the cartesian pose of the endEffectorFrame w.r.t to the baseFrame
+    // Note that all the target are defined implicitly w.r.t. to the world frame of the ik, but
+    // as we imposed that the transform between the baseFrame and the world is the identity,
+    // asking for a desired world_H_ee pose is equivalent to asking for a desired base_H_ee pose
+    ok = ik.addTarget(endEffectorFrame, base_H_ee_desired);
+    if (!ok) {
+        std::cerr << "iDynTreeExampleInverseKinematics: impossible to add target on  " << endEffectorFrame << std::endl;
+        return EXIT_FAILURE;
+    }
+
+    // As the IK optimization problem is a non-linear iterative optimization, the optimization needs
+    // an initial starting point, we use the initial position of the robot but note that this choice
+    // can influence the actual convergence of the algorithm
+    ik.setFullJointsInitialCondition(&world_H_base, &(jointPosInitialInRadians));
+
+    // Actually run the inverse kinematics optimization problem
+    ok = ik.solve();
+    if (!ok) {
+        std::cerr << "iDynTreeExampleInverseKinematics: impossible to solve inverse kinematics problem." << std::endl;
+        return EXIT_FAILURE;
+    }
+
+    // Read the solution
+    iDynTree::Transform basePoseOptimized;
+    iDynTree::VectorDynSize jointPosOptimizedInRadians(mdlLoader.model().getNrOfPosCoords());
+
+    ik.getFullJointsSolution(basePoseOptimized, jointPosOptimizedInRadians);
+
+    // Compute the actual transform associated with the optimized values
+    kinDynMeas.setJointPos(jointPosOptimizedInRadians);
+    iDynTree::Transform base_H_ee_optimized = kinDynMeas.getRelativeTransform(baseFrame, endEffectorFrame);
+
+    std::cerr << "Optimized cartesian position: " << std::endl << "\t" << base_H_ee_optimized.getPosition().toString() << std::endl;
+    std::cerr << "Optimized joint position (radians): " << std::endl << "\t" << jointPosOptimizedInRadians.toString() << std::endl;
+
+    return EXIT_SUCCESS;
+}
+

From 85aeb7d63c495add3977d8adc4b9abcb54701efa Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 4 Nov 2018 21:37:29 +0100
Subject: [PATCH 061/194] Added visualization of vectors.

Fix of few memory leaks related to visualization.
---
 src/visualization/CMakeLists.txt              |   2 +
 .../include/iDynTree/Visualizer.h             |  70 ++++++
 src/visualization/src/DummyImplementations.h  |  13 +
 src/visualization/src/Environment.cpp         |  17 +-
 src/visualization/src/IrrlichtUtils.h         |   4 +
 .../src/VectorsVisualization.cpp              | 224 ++++++++++++++++++
 src/visualization/src/VectorsVisualization.h  |  77 ++++++
 src/visualization/src/Visualizer.cpp          |  32 ++-
 .../tests/VisualizerUnitTest.cpp              |  34 +++
 9 files changed, 464 insertions(+), 9 deletions(-)
 create mode 100644 src/visualization/src/VectorsVisualization.cpp
 create mode 100644 src/visualization/src/VectorsVisualization.h

diff --git a/src/visualization/CMakeLists.txt b/src/visualization/CMakeLists.txt
index fb5f0024f51..cf844769cdd 100644
--- a/src/visualization/CMakeLists.txt
+++ b/src/visualization/CMakeLists.txt
@@ -15,12 +15,14 @@ if(IDYNTREE_USES_IRRLICHT)
                                                src/Environment.h
                                                src/ModelVisualization.h
                                                src/JetsVisualization.h
+                                               src/VectorsVisualization.h
                                                src/Light.h
                                                src/FloorGridSceneNode.h)
     set(iDynTree_visualization_private_source  src/Camera.cpp
                                                src/Environment.cpp
                                                src/ModelVisualization.cpp
                                                src/JetsVisualization.cpp
+                                               src/VectorsVisualization.cpp
                                                src/Light.cpp
                                                src/FloorGridSceneNode.cpp)
 endif()
diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index fbf5d17dad9..0a1ce1b917c 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -14,6 +14,7 @@
 #include <string>
 
 #include <iDynTree/Core/Direction.h>
+#include <iDynTree/Core/Position.h>
 
 #include <iDynTree/Model/JointState.h>
 #include <iDynTree/Model/LinkState.h>
@@ -293,6 +294,70 @@ class IJetsVisualization
     virtual bool setJetsIntensity(const VectorDynSize & jetsIntensity) = 0;
 };
 
+/**
+ * Interface to the visualization of vectors.
+ */
+class IVectorsVisualization
+{
+public:
+    /**
+     * Denstructor
+     */
+    virtual ~IVectorsVisualization() = 0;
+
+    /**
+     * @brief Add a vector in the visualization
+     * @return The vector index.
+     */
+    virtual size_t addVector(const Position & origin, const Direction & direction, double modulus) = 0;
+
+    /**
+     * @brief Add a vector in the visualization
+     * @return The vector index.
+     */
+    virtual size_t addVector(const Position & origin, const Vector3 & components) = 0;
+
+    /**
+     * Get the number of visualized vectors.
+     *
+     */
+    virtual size_t getNrOfVectors() const = 0;
+
+    /**
+     * Get vector properties.
+     */
+    virtual bool getVector(size_t vectorIndex, Position & currentOrigin,
+                           Direction & currentDirection, double & currentModulus) const = 0;
+
+    /**
+     * Get vector properties.
+     */
+    virtual bool getVector(size_t vectorIndex, Position & currentOrigin, Vector3 & components) const = 0;
+
+    /**
+     * Update Vector
+     */
+    virtual bool updateVector(size_t vectorIndex, const Position & origin, const Direction & direction, double modulus) = 0;
+
+    /**
+     * Update Vector
+     */
+    virtual bool updateVector(size_t vectorIndex, const Position & origin, const Vector3& components) = 0;
+
+    /**
+     * Set vector color.
+     */
+    virtual bool setVectorColor(size_t vectorIndex, const ColorViz & vectorColor) = 0;
+
+    /**
+     * @brief Determines the dimension of the visualized arrows
+     * @param zeroModulusRadius Constant offset for the arrow radius.
+     * @param modulusMultiplier Multiplies the modulus and adds up to the zeroModulusRadius to get the total arrow radius.
+     * @return true if successfull, false in case of negative numbers.
+     */
+    virtual bool setVectorsAspect(double zeroModulusRadius, double modulusMultiplier, double heightScale) = 0;
+};
+
 
 /**
  * Interface to the visualization of a model istance.
@@ -485,6 +550,11 @@ friend class ModelVisualization;
      */
     IEnvironment& enviroment();
 
+    /**
+     * Get a reference to the internal IVectorsVisualization interface.
+     */
+    IVectorsVisualization& vectors();
+
     /**
      * Wrap the run method of the Irrlicht device.
      */
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index e2b13980211..64abc199e31 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -89,6 +89,19 @@ class DummyJetsVisualization : public IJetsVisualization
     virtual bool setJetsIntensity(const VectorDynSize & ) { return false; };
 };
 
+class DummyVectorsVisualization : public IVectorsVisualization {
+public:
+    virtual ~DummyVectorsVisualization() override { }
+    virtual size_t addVector(const Position &, const Direction &, double) override { return 0; }
+    virtual size_t addVector(const Position &, const Vector3 &) override { return 0; }
+    virtual size_t getNrOfVectors() const override { return 0; }
+    virtual bool getVector(size_t, Position &, Direction &, double &) const override { return false; }
+    virtual bool getVector(size_t, Position &, Vector3 &) const override { return false; }
+    virtual bool updateVector(size_t , const Position &, const Direction &, double) override { return false; }
+    virtual bool updateVector(size_t, const Position &, const Vector3&) override { return false; }
+    virtual bool setVectorColor(size_t , const ColorViz &) override { return false; }
+    virtual bool setVectorsAspect(double, double, double) override { return false; }
+};
 
 /**
  * Dummy model visualization.
diff --git a/src/visualization/src/Environment.cpp b/src/visualization/src/Environment.cpp
index a5614122046..ee5c2539396 100644
--- a/src/visualization/src/Environment.cpp
+++ b/src/visualization/src/Environment.cpp
@@ -14,20 +14,25 @@
 namespace iDynTree
 {
 
-Environment::Environment(): m_sceneManager(0),
-                            m_rootFrameNode(0),
-                            m_floorGridNode(0)
+Environment::Environment(): m_sceneManager(nullptr),
+                            m_rootFrameNode(nullptr),
+                            m_floorGridNode(nullptr)
 {
 
 }
 
 void Environment::close()
 {
+    if (m_floorGridNode){
+        m_floorGridNode->drop();
+        m_floorGridNode = nullptr;
+    }
+
     for(size_t i=0; i < m_lights.size(); i++)
     {
         m_lights[i]->removeLight();
         delete m_lights[i];
-        m_lights[i] = 0;
+        m_lights[i] = nullptr;
     }
 
     m_lights.resize(0);
@@ -108,7 +113,7 @@ bool Environment::addLight(const std::string& lightName)
         }
     }
 
-    int lightIdx = m_lights.size();
+    size_t lightIdx = m_lights.size();
 
     // Add a new light
     m_lights.push_back(new Light());
@@ -141,7 +146,7 @@ bool Environment::removeLight(const std::string& lightName)
         {
             m_lights[i]->removeLight();
             delete m_lights[i];
-            m_lights[i] = 0;
+            m_lights[i] = nullptr;
             std::vector<Light*>::iterator it;
             std::advance(it,i);
             m_lights.erase(it);
diff --git a/src/visualization/src/IrrlichtUtils.h b/src/visualization/src/IrrlichtUtils.h
index c181567c73e..d4d4c2f0168 100644
--- a/src/visualization/src/IrrlichtUtils.h
+++ b/src/visualization/src/IrrlichtUtils.h
@@ -160,6 +160,8 @@ inline irr::scene::ISceneNode * addGeometryToSceneManager(const iDynTree::SolidS
 
         geomNode = smgr->addMeshSceneNode(boxMesh,linkNode);
 
+        boxMesh->drop();
+
     }
 
     if (geom->isSphere())
@@ -284,6 +286,8 @@ inline irr::scene::ISceneNode * addFrameAxes(irr::scene::ISceneManager* smgr,
     zArrow->getMaterial(0) = transBlue;
     zArrow->getMaterial(1) = transBlue;
 
+    arrowMesh->drop();
+
     return frameNode;
 }
 
diff --git a/src/visualization/src/VectorsVisualization.cpp b/src/visualization/src/VectorsVisualization.cpp
new file mode 100644
index 00000000000..2617b09169b
--- /dev/null
+++ b/src/visualization/src/VectorsVisualization.cpp
@@ -0,0 +1,224 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include "VectorsVisualization.h"
+
+#include "IrrlichtUtils.h"
+
+#include <cassert>
+#include <string>
+#include <cmath>
+
+using namespace iDynTree;
+
+
+
+void VectorsVisualization::drawVector(size_t vectorIndex)
+{
+    // Delete existing node
+    if (m_vectors[vectorIndex].visualizationNode)
+    {
+        m_vectors[vectorIndex].visualizationNode->remove();
+        m_vectors[vectorIndex].visualizationNode = nullptr;
+    }
+
+    float arrowHeight = static_cast<float>(std::abs(m_vectors[vectorIndex].modulus) * m_heightScale);
+    float arrowWidth = static_cast<float>(m_radiusOffset +
+                                          m_radiusMultiplier * std::abs(m_vectors[vectorIndex].modulus) * m_heightScale);
+
+
+    auto arrowPosition = idyntree2irr_pos(m_vectors[vectorIndex].origin);
+
+    iDynTree::Direction vectorDirection = (m_vectors[vectorIndex].modulus < 0) ?
+                m_vectors[vectorIndex].direction.reverse() :
+                m_vectors[vectorIndex].direction;
+
+    iDynTree::Direction yDirection(0.0, 1.0, 0.0); //Arrows in irrlicht are pointing in the y direction by default
+
+    iDynTree::Direction rotationAxis;
+
+    iDynTree::toEigen(rotationAxis) = iDynTree::toEigen(yDirection).cross(iDynTree::toEigen(vectorDirection));
+    rotationAxis.Normalize();
+
+    double rotationAngle = std::acos(iDynTree::toEigen(vectorDirection).dot(iDynTree::toEigen(yDirection)));
+
+    iDynTree::Rotation arrowRotationMatrix = iDynTree::Rotation::RotAxis(rotationAxis, rotationAngle);
+
+    irr::core::vector3df arrowRotation = idyntree2irr_rot(arrowRotationMatrix);
+
+    irr::scene::ISceneNode * frameNode = m_smgr->addEmptySceneNode();
+
+    irr::scene::IMesh* arrowMesh = m_smgr->getGeometryCreator()->createArrowMesh(4, 8, arrowHeight, 0.9f * arrowHeight,
+                                                                                 arrowWidth, 2.0f * arrowWidth);
+
+    m_vectors[vectorIndex].visualizationNode = m_smgr->addMeshSceneNode(arrowMesh,frameNode);
+    m_vectors[vectorIndex].visualizationNode->setPosition(arrowPosition);
+    m_vectors[vectorIndex].visualizationNode->setRotation(arrowRotation);
+    irr::video::SMaterial arrowColor;
+    arrowColor.AmbientColor = idyntree2irrlicht(m_vectors[vectorIndex].color).toSColor();
+    arrowColor.DiffuseColor = arrowColor.AmbientColor;
+    m_vectors[vectorIndex].visualizationNode->getMaterial(0) = arrowColor;
+    m_vectors[vectorIndex].visualizationNode->getMaterial(1) = arrowColor;
+
+
+    arrowMesh->drop();
+    arrowMesh = nullptr;
+}
+
+void VectorsVisualization::drawAll()
+{
+    for (size_t i = 0; i < m_vectors.size(); ++i) {
+        drawVector(i);
+    }
+}
+
+VectorsVisualization::VectorsVisualization()
+    : m_smgr(nullptr)
+    , m_radiusOffset(0.01)
+    , m_radiusMultiplier(0.0)
+    , m_heightScale(1.0)
+{
+
+}
+
+void VectorsVisualization::init(irr::scene::ISceneManager *smgr)
+{
+    assert(smgr);
+    m_smgr = smgr;
+}
+
+void VectorsVisualization::close()
+{
+    for (auto& vector: m_vectors) {
+        if (vector.visualizationNode) {
+            vector.visualizationNode->removeAll();
+            //vector.visualizationNode->drop();
+            vector.visualizationNode = nullptr;
+        }
+    }
+    m_vectors.resize(0);
+}
+
+VectorsVisualization::~VectorsVisualization()
+{
+    close();
+}
+
+size_t VectorsVisualization::addVector(const Position &origin, const Direction &direction, double modulus)
+{
+    VectorsProperties newVector;
+    newVector.origin = origin;
+    newVector.direction = direction;
+    newVector.modulus = modulus;
+
+    m_vectors.push_back(newVector);
+
+    drawVector(m_vectors.size()-1);
+
+    return m_vectors.size() - 1;
+}
+
+size_t VectorsVisualization::addVector(const Position &origin, const Vector3 &components)
+{
+    return addVector(origin, Direction(components(0), components(1), components(2)), toEigen(components).norm());
+}
+
+size_t VectorsVisualization::getNrOfVectors() const
+{
+    return m_vectors.size();
+}
+
+bool VectorsVisualization::getVector(size_t vectorIndex, Position &currentOrigin, Direction &currentDirection, double &currentModulus) const
+{
+    if (vectorIndex >= m_vectors.size()) {
+        reportError("VectorsVisualization","getVector","vectorIndex out of bounds.");
+        return false;
+    }
+
+    currentDirection = m_vectors[vectorIndex].direction;
+    currentOrigin = m_vectors[vectorIndex].origin;
+    currentModulus = m_vectors[vectorIndex].modulus;
+
+    return true;
+}
+
+bool VectorsVisualization::getVector(size_t vectorIndex, Position &currentOrigin, Vector3 &components) const
+{
+    if (vectorIndex >= m_vectors.size()) {
+        reportError("VectorsVisualization","getVector","vectorIndex out of bounds.");
+        return false;
+    }
+
+    currentOrigin = m_vectors[vectorIndex].origin;
+    toEigen(components) = m_vectors[vectorIndex].modulus * toEigen(m_vectors[vectorIndex].direction);
+
+    return true;
+}
+
+bool VectorsVisualization::updateVector(size_t vectorIndex, const Position &origin, const Direction &direction, double modulus)
+{
+    if (vectorIndex >= m_vectors.size()) {
+        reportError("VectorsVisualization","updateVector","vectorIndex out of bounds.");
+        return false;
+    }
+
+    m_vectors[vectorIndex].origin = origin;
+    m_vectors[vectorIndex].direction = direction;
+    m_vectors[vectorIndex].modulus = modulus;
+
+    drawVector(vectorIndex);
+
+    return true;
+}
+
+bool VectorsVisualization::updateVector(size_t vectorIndex, const Position &origin, const Vector3 &components)
+{
+    return updateVector(vectorIndex, origin, Direction(components(0), components(1), components(2)), toEigen(components).norm());
+}
+
+bool VectorsVisualization::setVectorColor(size_t vectorIndex, const ColorViz &vectorColor)
+{
+    if (vectorIndex >= m_vectors.size()) {
+        reportError("VectorsVisualization","setVectorColor","vectorIndex out of bounds.");
+        return false;
+    }
+
+    m_vectors[vectorIndex].color = vectorColor;
+
+    drawVector(vectorIndex);
+
+    return true;
+}
+
+bool VectorsVisualization::setVectorsAspect(double zeroModulusRadius, double modulusMultiplier, double heightScale)
+{
+    if (zeroModulusRadius < 0) {
+        reportError("VectorsVisualization","setVectorsAspect","zeroModulusRadius is supposed to be non negative.");
+        return false;
+    }
+
+    if (modulusMultiplier < 0) {
+        reportError("VectorsVisualization","setVectorsAspect","modulusMultiplier is supposed to be non negative.");
+        return false;
+    }
+
+    if (heightScale < 0) {
+        reportError("VectorsVisualization","setVectorsAspect","heightScale is supposed to be non negative.");
+        return false;
+    }
+
+    m_radiusOffset = zeroModulusRadius;
+    m_radiusMultiplier = modulusMultiplier;
+    m_heightScale = heightScale;
+
+    drawAll();
+
+    return true;
+}
diff --git a/src/visualization/src/VectorsVisualization.h b/src/visualization/src/VectorsVisualization.h
new file mode 100644
index 00000000000..0fe380a2ac4
--- /dev/null
+++ b/src/visualization/src/VectorsVisualization.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+#ifndef IDYNTREE_VECTORS_VISUALIZATION_H
+#define IDYNTREE_VECTORS_VISUALIZATION_H
+
+#include <iDynTree/Visualizer.h>
+
+#include <vector>
+#include <irrlicht.h>
+
+namespace iDynTree {
+
+    class VectorsVisualization : public IVectorsVisualization {
+
+        typedef struct {
+            Position origin;
+            Direction direction;
+            double modulus;
+            ColorViz color = ColorViz(1.0, 0.0, 0.0, 1.0);
+            irr::scene::ISceneNode * visualizationNode = nullptr;
+        } VectorsProperties;
+
+        std::vector<VectorsProperties> m_vectors;
+
+        irr::scene::ISceneManager* m_smgr;
+
+        double m_radiusOffset, m_radiusMultiplier, m_heightScale;
+
+        void drawVector(size_t vectorIndex);
+
+        void drawAll();
+
+    public:
+
+        VectorsVisualization();
+
+        VectorsVisualization(const VectorsVisualization& other) = delete;
+
+        VectorsVisualization& operator=(const VectorsVisualization& other) = delete;
+
+        void init(irr::scene::ISceneManager* smgr);
+
+        void close();
+
+        virtual ~VectorsVisualization() override;
+
+        virtual size_t addVector(const Position & origin, const Direction & direction, double modulus) override;
+
+        virtual size_t addVector(const Position & origin, const Vector3 & components) override;
+
+        virtual size_t getNrOfVectors() const override;
+
+        virtual bool getVector(size_t vectorIndex, Position & currentOrigin,
+                               Direction & currentDirection, double & currentModulus) const override;
+
+        virtual bool getVector(size_t vectorIndex, Position & currentOrigin, Vector3 & components) const override;
+
+        virtual bool updateVector(size_t vectorIndex, const Position & origin, const Direction & direction, double modulus) override;
+
+        virtual bool updateVector(size_t vectorIndex, const Position & origin, const Vector3& components) override;
+
+        virtual bool setVectorColor(size_t vectorIndex, const ColorViz & vectorColor) override;
+
+        virtual bool setVectorsAspect(double zeroModulusRadius, double modulusMultiplier, double heightScale) override;
+
+    };
+
+}
+
+#endif // IDYNTREE_VECTORS_VISUALIZATION_H
diff --git a/src/visualization/src/Visualizer.cpp b/src/visualization/src/Visualizer.cpp
index ffff4cfe100..0ace407fd67 100644
--- a/src/visualization/src/Visualizer.cpp
+++ b/src/visualization/src/Visualizer.cpp
@@ -18,6 +18,7 @@
 #include "Camera.h"
 #include "Environment.h"
 #include "ModelVisualization.h"
+#include "VectorsVisualization.h"
 #endif
 
 #include "DummyImplementations.h"
@@ -40,6 +41,9 @@ IJetsVisualization::~IJetsVisualization()
 
 }
 
+IVectorsVisualization::~IVectorsVisualization()
+{
+}
 
 IModelVisualization::~IModelVisualization()
 {
@@ -112,9 +116,15 @@ struct Visualizer::VisualizerPimpl
      */
     Environment m_environment;
 
+    /**
+     * Vectors visualization
+     */
+    VectorsVisualization m_vectors;
+
 #else
     DummyCamera m_camera;
     DummyEnvironment m_environment;
+    DummyVectorsVisualization m_invalidVectors;
 #endif
 
     VisualizerPimpl()
@@ -222,6 +232,8 @@ bool Visualizer::init(const VisualizerOptions options)
 
     pimpl->m_camera.setIrrlichtCamera(addVizCamera(pimpl->m_irrSmgr));
 
+    pimpl->m_vectors.init(pimpl->m_irrSmgr);
+
     pimpl->m_isInitialized = true;
     pimpl->lastFPS         = -1;
 
@@ -421,6 +433,19 @@ IEnvironment& Visualizer::enviroment()
     return pimpl->m_environment;
 }
 
+IVectorsVisualization &Visualizer::vectors()
+{
+#ifdef IDYNTREE_USES_IRRLICHT
+    if( !this->pimpl->m_isInitialized )
+    {
+        init();
+    }
+    return this->pimpl->m_vectors;
+#else
+    return this->pimpl->m_invalidVectors;
+#endif
+}
+
 bool Visualizer::run()
 {
 #ifdef IDYNTREE_USES_IRRLICHT
@@ -432,7 +457,7 @@ bool Visualizer::run()
 
     return pimpl->m_irrDevice->run();
 #else
-    reportError("Visualizer","init","Impossible to use iDynTree::Visualizer, as iDynTree has been compiled without Irrlicht.");
+    reportError("Visualizer","run","Impossible to use iDynTree::Visualizer, as iDynTree has been compiled without Irrlicht.");
     return false;
 #endif
 }
@@ -445,11 +470,12 @@ void Visualizer::close()
         return;
     }
 
+    pimpl->m_vectors.close();
     pimpl->m_environment.close();
 
     pimpl->m_irrDevice->closeDevice();
     pimpl->m_irrDevice->drop();
-    pimpl->m_irrDevice = 0;
+    pimpl->m_irrDevice = nullptr;
     pimpl->m_isInitialized = false;
 
     for(size_t mdl=0; mdl < pimpl->m_modelViz.size(); mdl++)
@@ -457,7 +483,7 @@ void Visualizer::close()
         if( pimpl->m_modelViz[mdl] )
         {
             delete pimpl->m_modelViz[mdl];
-            pimpl->m_modelViz[mdl] = 0;
+            pimpl->m_modelViz[mdl] = nullptr;
         }
     }
 
diff --git a/src/visualization/tests/VisualizerUnitTest.cpp b/src/visualization/tests/VisualizerUnitTest.cpp
index f2d10bdb559..7efb0e20c08 100644
--- a/src/visualization/tests/VisualizerUnitTest.cpp
+++ b/src/visualization/tests/VisualizerUnitTest.cpp
@@ -53,9 +53,43 @@ void threeLinksReducedTest()
     checkVizLoading(mdlLoaderReduced.model());
 }
 
+void checkArrowsVisualization() {
+    iDynTree::Visualizer viz;
+
+    iDynTree::IVectorsVisualization& vectors = viz.vectors();
+
+    size_t index = vectors.addVector(iDynTree::Position(0.1, 0.1, 0.0), iDynTree::Direction(0.0, -1.0, 0.0), 0.5);
+    ASSERT_IS_TRUE(index >= 0);
+
+    iDynTree::Vector3 components;
+    components(0) = 0.2;
+    components(1) = -0.5;
+    components(2) = 0.1;
+
+    index = vectors.addVector(iDynTree::Position(0.2, 0.1, 0.1), components);
+    ASSERT_IS_TRUE(index >= 0);
+    bool ok = vectors.setVectorColor(index, iDynTree::ColorViz(0.0, 1.0, 0.0, 1.0));
+    ASSERT_IS_TRUE(ok);
+    components(0) = 0.5;
+    components(1) = 0.0;
+    components(2) = 0.1;
+    ok = vectors.updateVector(index, iDynTree::Position(0.2, 0.1, 0.1), components);
+    ASSERT_IS_TRUE(ok);
+
+
+
+    for(int i=0; i < 5; i++)
+    {
+        viz.draw();
+    }
+
+    viz.close();
+}
+
 int main()
 {
     threeLinksReducedTest();
+    checkArrowsVisualization();
 
     return EXIT_SUCCESS;
 }

From 2b026e3268a96df2cce03e8a9d75a8d4f8e8b14f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 5 Nov 2018 09:20:58 +0100
Subject: [PATCH 062/194] Updated release notes.

---
 doc/releases/v0_12.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 0f4dffbfc7e..763038e0937 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -26,3 +26,6 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Fixed bugs in ``MultipleShooting`` solver.
 * Added few lines of documentation.
 
+#### `visualization`
+* Added visualization of vectors.
+

From 5747513351c1ae4e6ceb09658a902b0f8597a4c6 Mon Sep 17 00:00:00 2001
From: icub <icub@example.com>
Date: Wed, 7 Nov 2018 15:02:49 +0100
Subject: [PATCH 063/194] Fix Windows compilation issues

Fix compilation issues when IRRLICHT is used in Windows.
---
 src/visualization/CMakeLists.txt      | 3 +++
 src/visualization/src/IrrlichtUtils.h | 6 ++++--
 2 files changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/visualization/CMakeLists.txt b/src/visualization/CMakeLists.txt
index cf844769cdd..09aae9b65bc 100644
--- a/src/visualization/CMakeLists.txt
+++ b/src/visualization/CMakeLists.txt
@@ -70,6 +70,9 @@ if(IDYNTREE_USES_IRRLICHT)
 
     target_include_directories(${libraryname} PRIVATE SYSTEM ${IRRLICHT_INCLUDE_DIR} ${OPENGL_INCLUDE_DIR})
     target_link_libraries(${libraryname} LINK_PRIVATE ${IRRLICHT_LIBRARY} ${OPENGL_LIBRARIES})
+    if(MSVC)
+       add_definitions(-D_USE_MATH_DEFINES)
+    endif()
     add_definitions(-DIDYNTREE_USES_IRRLICHT)
 
     # On Apple system, irrlicht uses some system libraries 
diff --git a/src/visualization/src/IrrlichtUtils.h b/src/visualization/src/IrrlichtUtils.h
index d4d4c2f0168..c06bdfc79c6 100644
--- a/src/visualization/src/IrrlichtUtils.h
+++ b/src/visualization/src/IrrlichtUtils.h
@@ -23,6 +23,8 @@
 
 #include "FloorGridSceneNode.h"
 
+#include <cmath>
+
 namespace iDynTree
 {
 
@@ -39,7 +41,7 @@ inline iDynTree::ColorViz  irrlicht2idyntree(irr::video::SColorf color)
 
 inline irr::core::vector3df idyntree2irr_rpy(const iDynTree::Vector3 & vecId)
 {
-    const double kRad2deg = 180/M_PI;
+    double kRad2deg = 180/M_PI;
     return irr::core::vector3df(kRad2deg*vecId(0),kRad2deg*vecId(1),kRad2deg*vecId(2));
 }
 
@@ -55,7 +57,7 @@ inline  iDynTree::Position irr2idyntree_pos(const irr::core::vector3df & vecIrr)
 
 inline iDynTree::Vector3 irr2idyntree_rpy(const irr::core::vector3df & vecIrr)
 {
-    const double kDeg2rad = M_PI/180;
+    double kDeg2rad = M_PI/180;
     iDynTree::Vector3 ret;
     ret(0) = kDeg2rad*vecIrr.X;
     ret(1) = kDeg2rad*vecIrr.Y;

From e68f71256c45c4dc7de05541bfe57a85cf36d5a1 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 8 Nov 2018 23:11:34 +0100
Subject: [PATCH 064/194] Remove not useful comment

---
 src/visualization/src/VectorsVisualization.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/visualization/src/VectorsVisualization.cpp b/src/visualization/src/VectorsVisualization.cpp
index 2617b09169b..0dc05266aa1 100644
--- a/src/visualization/src/VectorsVisualization.cpp
+++ b/src/visualization/src/VectorsVisualization.cpp
@@ -99,7 +99,6 @@ void VectorsVisualization::close()
     for (auto& vector: m_vectors) {
         if (vector.visualizationNode) {
             vector.visualizationNode->removeAll();
-            //vector.visualizationNode->drop();
             vector.visualizationNode = nullptr;
         }
     }

From 9b51e954473511d84ef1d0ea94152a0d2ce9cc65 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Sun, 23 Sep 2018 20:50:37 +0200
Subject: [PATCH 065/194] Added ALGLIB interface and WORHP interface.

Inherited from feature/OCNLPSOlvers branch.
---
 cmake/iDynTreeDependencies.cmake              |  12 +
 src/optimalcontrol/CMakeLists.txt             |  17 +
 .../iDynTree/Optimizers/AlglibInterface.h     |  88 ++
 .../iDynTree/Optimizers/WorhpInterface.h      |  81 ++
 src/optimalcontrol/src/AlglibInterface.cpp    | 474 ++++++++++
 .../src/AlglibInterfaceNotImplemented.cpp     |  85 ++
 .../src/MultipleShootingSolver.cpp            |  24 +
 src/optimalcontrol/src/WorhpInterface.cpp     | 830 ++++++++++++++++++
 .../src/WorhpInterfaceNotImplemented.cpp      | 120 +++
 .../tests/AlglibInterfaceTest.cpp             | 214 +++++
 src/optimalcontrol/tests/CMakeLists.txt       |   9 +-
 ...timizerTest.cpp => IpoptInterfaceTest.cpp} |   0
 .../tests/WorhpInterfaceTest.cpp              | 227 +++++
 13 files changed, 2180 insertions(+), 1 deletion(-)
 create mode 100644 src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
 create mode 100644 src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
 create mode 100644 src/optimalcontrol/src/AlglibInterface.cpp
 create mode 100644 src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp
 create mode 100644 src/optimalcontrol/src/WorhpInterface.cpp
 create mode 100644 src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
 create mode 100644 src/optimalcontrol/tests/AlglibInterfaceTest.cpp
 rename src/optimalcontrol/tests/{OptimizerTest.cpp => IpoptInterfaceTest.cpp} (100%)
 create mode 100644 src/optimalcontrol/tests/WorhpInterfaceTest.cpp

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 0529bbb9d8d..6696d2ae6b8 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -51,8 +51,20 @@ if(IDYNTREE_USES_YARP AND YARP_FOUND)
   endif()
 endif()
 
+find_library(WORHP_LIB worhp HINTS ENV WORHP_DIR)
+option(IDYNTREE_USES_WORHP "Use WORHP as a solver in optimalcontrol." WORHP_LIB)
+if (IDYNTREE_USES_WORHP)
+    if (NOT WORHP_LIB)
+        message(FATAL_ERROR "The IDYNTREE_USES_WORHP option was selected, but it was not possible to find WORHP.")
+    endif()
+
+    find_path(WORHP_INCLUDE_DIRS worhp HINTS ENV WORHP_DIR)
+
+endif()
+
 idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
+idyntree_handle_dependency(ALGLIB)
 
diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index bfcadef696c..d84bbb52070 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -90,6 +90,23 @@ else()
     list(APPEND OPTIMIZERS_SOURCES src/OsqpInterfaceNotImplemented.cpp)
 endif()
 
+list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/AlglibInterface.h)
+if (IDYNTREE_USES_ALGLIB)
+    list(APPEND OPTIMIZERS_SOURCES src/AlglibInterface.cpp)
+    list(APPEND LINK_LIST ${ALGLIB_LIB})
+    list(APPEND INCLUDE_LIST ${ALGLIB_INCLUDE_DIRS})
+else()
+    list(APPEND OPTIMIZERS_SOURCES src/AlglibInterfaceNotImplemented.cpp)
+endif()
+
+list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/WorhpInterface.h)
+if (IDYNTREE_USES_WORHP)
+    list(APPEND OPTIMIZERS_SOURCES src/WorhpInterface.cpp)
+    list(APPEND LINK_LIST ${WORHP_LIB})
+    list(APPEND INCLUDE_LIST ${WORHP_INCLUDE_DIRS})
+else()
+    list(APPEND OPTIMIZERS_SOURCES src/WorhpInterfaceNotImplemented.cpp)
+endif()
 
 add_library(${libraryname} ${PUBLIC_HEADERS} ${INTEGRATORS_PUBLIC_HEADERS} ${OCSOLVERS_PUBLIC_HEADERS} ${OPTIMIZERS_HEADERS} ${SOURCES} ${OPTIMIZERS_SOURCES})
 target_include_directories(${libraryname} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
new file mode 100644
index 00000000000..23e65f7ce20
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#ifndef IDYNTREE_OPTIMALCONTROL_ALGLIBINTERFACE_H
+#define IDYNTREE_OPTIMALCONTROL_ALGLIBINTERFACE_H
+
+#include <iDynTree/Optimizer.h>
+
+namespace iDynTree {
+
+    class VectorDynSize;
+
+    namespace optimization {
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+
+        class AlglibInterface : public Optimizer {
+
+            class AlglibInterfaceImplementation;
+            AlglibInterfaceImplementation *m_pimpl;
+
+        public:
+
+            AlglibInterface();
+
+            AlglibInterface(const AlglibInterface &other) = delete;
+
+            virtual ~AlglibInterface() override;
+
+            virtual bool isAvailable() const override;
+
+            virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
+
+            virtual bool solve() override;
+
+            virtual bool getPrimalVariables(VectorDynSize &primalVariables) override;
+
+            virtual bool getDualVariables(VectorDynSize &constraintsMultipliers,
+                                          VectorDynSize &lowerBoundsMultipliers,
+                                          VectorDynSize &upperBoundsMultipliers) override;
+
+            virtual bool getOptimalCost(double &optimalCost) override;
+
+            virtual bool getOptimalConstraintsValues(VectorDynSize &constraintsValues) override;
+
+            virtual double minusInfinity() override;
+
+            virtual double plusInfinity() override;
+
+            //Set the penalty coefficient that multiplies constraints value in the cost function of the correspondent uncostrained problem.
+            // See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlcsetalgoaul
+            bool setRHO(double rho);
+
+            //Number of iterations to refine Lagrange multipliers.
+            //See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlcsetalgoaul
+            bool setOuterIterations(unsigned int outerIterations);
+
+            //epsG: stopping criterion on the (scaled) cost gradient. Stops if its norm is lower than epsG.
+            //epsF: stopping criterion on the variation of the cost function
+            //epsX: stopping criterion on the variation of optimization variables
+            //See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlcsetcond
+            bool setStoppingConditions(double epsG, double epsF, double epsX);
+
+            bool setMaximumIterations(unsigned int maxIter);
+
+        };
+
+    }
+
+}
+
+#endif // ALGLIBINTERFACE_H
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
new file mode 100644
index 00000000000..059c2fa68b3
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#ifndef IDYNTREE_OPTIMALCONTROL_WORHPINTERFACE_H
+#define IDYNTREE_OPTIMALCONTROL_WORHPINTERFACE_H
+
+#include <iDynTree/Optimizer.h>
+
+namespace iDynTree {
+
+    class VectorDynSize;
+
+    namespace optimization {
+
+        /**
+         * @warning This class is still in active development, and so API interface can change between iDynTree versions.
+         * \ingroup iDynTreeExperimental
+         */
+
+        class WorhpInterface : public Optimizer {
+
+            class WorhpInterfaceImplementation;
+            WorhpInterfaceImplementation *m_pimpl;
+
+        public:
+
+            WorhpInterface();
+
+            WorhpInterface(const WorhpInterface &other) = delete;
+
+            virtual ~WorhpInterface() override;
+
+            virtual bool isAvailable() const override;
+
+            virtual bool setProblem(std::shared_ptr<OptimizationProblem> problem) override;
+
+            virtual bool solve() override;
+
+            virtual bool getPrimalVariables(VectorDynSize &primalVariables) override;
+
+            virtual bool getDualVariables(VectorDynSize &constraintsMultipliers,
+                                          VectorDynSize &lowerBoundsMultipliers,
+                                          VectorDynSize &upperBoundsMultipliers) override;
+
+            virtual bool getOptimalCost(double &optimalCost) override;
+
+            virtual bool getOptimalConstraintsValues(VectorDynSize &constraintsValues) override;
+
+            virtual double minusInfinity() override;
+
+            virtual double plusInfinity() override;
+
+            bool setWorhpParam(const std::string &paramName, bool value);
+
+            bool setWorhpParam(const std::string &paramName, double value);
+
+            bool setWorhpParam(const std::string &paramName, int value);
+
+            bool getWorhpParam(const std::string &paramName, bool &value);
+
+            bool getWorhpParam(const std::string &paramName, double &value);
+
+            bool getWorhpParam(const std::string &paramName, int &value);
+        };
+    }
+}
+
+#endif // IDYNTREE_OPTIMALCONTROL_WORHPINTERFACE_H
diff --git a/src/optimalcontrol/src/AlglibInterface.cpp b/src/optimalcontrol/src/AlglibInterface.cpp
new file mode 100644
index 00000000000..f292d3cc8d4
--- /dev/null
+++ b/src/optimalcontrol/src/AlglibInterface.cpp
@@ -0,0 +1,474 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/AlglibInterface.h>
+
+#include <Eigen/Dense>
+#include <iDynTree/Core/EigenHelpers.h>
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include "iDynTree/Core/Utils.h"
+
+#include <optimization.h>
+#include <stdafx.h>
+
+#include <cassert>
+#include <map>
+
+namespace iDynTree {
+
+    namespace optimization {
+
+        typedef struct{
+            double boundValue;
+            double constraintSign;
+            unsigned int index;
+        }ConstraintInfo;
+
+        typedef struct{
+            std::shared_ptr<OptimizationProblem> problem;
+            std::map<size_t, ConstraintInfo> expandedToOriginalInequalities, expandedToOriginalEqualities;
+            VectorDynSize variablesBuffer, costJacobianBuffer, constraintsBuffer;
+            MatrixDynSize constraintJacobian, zeroMatrix, jacBuffer;
+        }SharedData;
+
+        class AlglibInterface::AlglibInterfaceImplementation {
+        public:
+            SharedData* nlpData;
+            VectorDynSize constraintsLowerBounds, constraintsUpperBounds, variablesLowerBounds, variablesUpperBounds, x0, solution;
+            alglib::minnlcstate alglibState;
+            alglib::minnlcreport alglibReport;
+            alglib::real_1d_array alglibScaling;
+            int exitCode;
+            double epsg;
+            double epsf;
+            double epsx;
+            alglib::ae_int_t maxits;
+            alglib::ae_int_t outerits;
+            double rho;
+
+
+            AlglibInterfaceImplementation(){
+                nlpData = new(SharedData);
+                assert(nlpData);
+                nlpData->problem = nullptr;
+                exitCode = -10;
+                epsg = 0;
+                epsf = 0;
+                epsx = 0.000001;
+                maxits = 0;
+                outerits = 5;
+                rho = 1000;
+            }
+
+        };
+
+        void ALGLIB_NLP(const alglib::real_1d_array &x, alglib::real_1d_array &fi, alglib::real_2d_array &jac, void *ptr){
+            // See http://www.alglib.net/translator/man/manual.cpp.html#example_minnlc_d_inequality
+            SharedData* pimpl = (SharedData*) ptr;
+            assert(pimpl->problem);
+            Eigen::Map<const Eigen::VectorXd> x_map(x.getcontent(), x.length());
+            Eigen::Map<Eigen::VectorXd> f_map(fi.getcontent(), fi.length());
+
+            assert(x_map.size() == pimpl->problem->numberOfVariables());
+            assert(f_map.size() == pimpl->expandedToOriginalInequalities.size() + pimpl->expandedToOriginalEqualities.size() + 1); //cost function + constraints
+            assert((jac.rows() == f_map.size()) && (jac.cols() == x_map.size()));
+            assert((jac.rows() == pimpl->jacBuffer.rows()) && (jac.cols() == pimpl->jacBuffer.cols()));
+
+            toEigen(pimpl->variablesBuffer) = x_map;
+
+            if (!(pimpl->problem->setVariables(pimpl->variablesBuffer))){
+                reportError("ALGLIBInterface", "solve", "Error while setting the optimizatin variables to the problem.");
+                assert(false);
+            }
+
+            double cost = 0;
+            if (!(pimpl->problem->evaluateCostFunction(cost))){
+                reportError("ALGLIBInterface", "solve", "Error while evaluating the cost function.");
+                assert(false);
+            }
+
+            if (!(pimpl->problem->evaluateCostGradient(pimpl->costJacobianBuffer))){
+                reportError("ALGLIBInterface", "solve", "Error while retrieving the cost gradient.");
+                assert(false);
+            }
+
+            if (!(pimpl->problem->evaluateConstraints(pimpl->constraintsBuffer))){
+                reportError("ALGLIBInterface", "solve", "Error while evaluating the constraints.");
+                assert(false);
+            }
+
+            pimpl->constraintJacobian = pimpl->zeroMatrix; //initialization for nonzero elements
+
+            if (!(pimpl->problem->evaluateConstraintsJacobian(pimpl->constraintJacobian))){
+                reportError("ALGLIBInterface", "solve", "Error while evaluating the constraints jacobian.");
+                assert(false);
+            }
+
+            f_map(0) = cost;
+            toEigen(pimpl->jacBuffer).row(0) = toEigen(pimpl->costJacobianBuffer);
+            long index = 1;
+            ConstraintInfo constraint;
+            for (size_t i = 0; i < pimpl->expandedToOriginalEqualities.size(); ++i){
+                constraint = pimpl->expandedToOriginalEqualities[i];
+                f_map(index) = constraint.boundValue + constraint.constraintSign * pimpl->constraintsBuffer(constraint.index);
+                toEigen(pimpl->jacBuffer).row(index) = constraint.constraintSign * toEigen(pimpl->constraintJacobian).row(constraint.index);
+                index++;
+            }
+
+            for (size_t i = 0; i < pimpl->expandedToOriginalInequalities.size(); ++i){
+                constraint = pimpl->expandedToOriginalInequalities[i];
+                f_map(index) = constraint.boundValue + constraint.constraintSign * pimpl->constraintsBuffer(constraint.index);
+                toEigen(pimpl->jacBuffer).row(index) = constraint.constraintSign * toEigen(pimpl->constraintJacobian).row(constraint.index);
+                index++;
+            }
+
+            alglib::real_2d_array tempAlglibMatrix;
+            tempAlglibMatrix.attach_to_ptr(pimpl->jacBuffer.rows(), pimpl->jacBuffer.cols(), pimpl->jacBuffer.data());
+            jac = tempAlglibMatrix;
+
+        }
+
+
+
+
+        AlglibInterface::AlglibInterface()
+        :m_pimpl(new AlglibInterfaceImplementation())
+        {
+            assert(m_pimpl);
+        }
+
+        AlglibInterface::~AlglibInterface()
+        {
+            if (m_pimpl){
+                if (m_pimpl->nlpData){
+                    delete m_pimpl->nlpData;
+                    m_pimpl->nlpData = nullptr;
+                }
+                delete m_pimpl;
+                m_pimpl = nullptr;
+            }
+
+        }
+
+        bool AlglibInterface::isAvailable() const
+        {
+            return true;
+        }
+
+        bool AlglibInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+        {
+            if (!problem){
+                reportError("ALGLIBInterface", "setProblem", "Empty problem pointer.");
+                return false;
+            }
+            m_pimpl->nlpData->problem = problem;
+            return true;
+        }
+
+        bool AlglibInterface::solve()
+        {
+            if (!(m_pimpl->nlpData->problem)){
+                reportError("ALGLIBInterface", "solve", "First you have to set the problem.");
+                return false;
+            }
+
+            if (!(m_pimpl->nlpData->problem->prepare())){
+                reportError("ALGLIBInterface", "solve", "Failed while preparing the optimization problem.");
+                return false;
+            }
+
+            unsigned int nx = m_pimpl->nlpData->problem->numberOfVariables();
+            unsigned int nc = m_pimpl->nlpData->problem->numberOfConstraints();
+
+            if (m_pimpl->nlpData->variablesBuffer.size() != nx)
+                m_pimpl->nlpData->variablesBuffer.resize(nx);
+
+            if (m_pimpl->solution.size() != nx)
+                m_pimpl->solution.resize(nx);
+
+            if (m_pimpl->nlpData->constraintsBuffer.size() != nc)
+                m_pimpl->nlpData->constraintsBuffer.resize(nc);
+
+            if (m_pimpl->nlpData->costJacobianBuffer.size() != nx)
+                m_pimpl->nlpData->costJacobianBuffer.resize(nx);
+
+            if ((m_pimpl->nlpData->constraintJacobian.rows() != nc) || (m_pimpl->nlpData->constraintJacobian.cols() != nx))
+                m_pimpl->nlpData->constraintJacobian.resize(nc, nx);
+
+            if ((m_pimpl->nlpData->zeroMatrix.rows() != nc) || (m_pimpl->nlpData->zeroMatrix.cols() != nx)){
+                m_pimpl->nlpData->zeroMatrix.resize(nc, nx);
+                m_pimpl->nlpData->zeroMatrix.zero();
+            }
+
+            if (m_pimpl->variablesLowerBounds.size() != nx)
+                m_pimpl->variablesLowerBounds.resize(nx);
+
+            if (!(m_pimpl->nlpData->problem->getVariablesLowerBound(m_pimpl->variablesLowerBounds)))
+                toEigen(m_pimpl->variablesLowerBounds).setConstant(minusInfinity());
+
+            if (m_pimpl->variablesUpperBounds.size() != nx)
+                m_pimpl->variablesUpperBounds.resize(nx);
+
+            if (!(m_pimpl->nlpData->problem->getVariablesUpperBound(m_pimpl->variablesUpperBounds)))
+                toEigen(m_pimpl->variablesUpperBounds).setConstant(plusInfinity());
+
+            if (m_pimpl->constraintsUpperBounds.size() != nc)
+                m_pimpl->constraintsUpperBounds.resize(nc);
+
+            if (m_pimpl->constraintsLowerBounds.size() != nc)
+                m_pimpl->constraintsLowerBounds.resize(nc);
+
+            if (!(m_pimpl->nlpData->problem->getConstraintsBounds(m_pimpl->constraintsLowerBounds, m_pimpl->constraintsUpperBounds))){
+                reportError("ALGLIBInterface", "solve", "Error while retrieving the constraints bounds.");
+                return false;
+            }
+
+            unsigned int inequalities = 0, equalities = 0;
+            ConstraintInfo newConstraint;
+            for (unsigned int i=0; i < nc; ++i){
+                if (m_pimpl->constraintsLowerBounds(i) == m_pimpl->constraintsUpperBounds(i)){
+                    newConstraint.boundValue = -1.0 * m_pimpl->constraintsUpperBounds(i);
+                    newConstraint.constraintSign = +1.0;
+                    newConstraint.index = i;
+                    m_pimpl->nlpData->expandedToOriginalEqualities[equalities] = newConstraint;
+                    equalities++;
+                } else {
+
+                    if (m_pimpl->constraintsLowerBounds(i) > minusInfinity()){
+                        newConstraint.boundValue = m_pimpl->constraintsLowerBounds(i);
+                        newConstraint.constraintSign = -1.0;
+                        newConstraint.index = i;
+                        m_pimpl->nlpData->expandedToOriginalInequalities[inequalities] = newConstraint;
+                        inequalities++;
+                    }
+
+                    if (m_pimpl->constraintsUpperBounds(i) < plusInfinity()){
+                        newConstraint.boundValue = -1.0 * m_pimpl->constraintsUpperBounds(i);
+                        newConstraint.constraintSign = +1.0;
+                        newConstraint.index = i;
+                        m_pimpl->nlpData->expandedToOriginalInequalities[inequalities] = newConstraint;
+                        inequalities++;
+                    }
+                }
+            }
+            m_pimpl->nlpData->expandedToOriginalEqualities.erase(m_pimpl->nlpData->expandedToOriginalEqualities.find(equalities), m_pimpl->nlpData->expandedToOriginalEqualities.end());
+            m_pimpl->nlpData->expandedToOriginalInequalities.erase(m_pimpl->nlpData->expandedToOriginalInequalities.find(inequalities), m_pimpl->nlpData->expandedToOriginalInequalities.end());
+
+            if ((m_pimpl->nlpData->jacBuffer.rows() != (equalities + inequalities + 1)) || (m_pimpl->nlpData->jacBuffer.cols() != nx))
+                m_pimpl->nlpData->jacBuffer.resize((equalities + inequalities + 1), nx);
+
+            // See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlccreate
+
+            if (m_pimpl->x0.size() != nx){
+                m_pimpl->x0.resize(nx);
+                m_pimpl->x0.zero();
+            }
+
+            if (m_pimpl->nlpData->problem->getGuess(m_pimpl->x0)) {
+                if (m_pimpl->x0.size() != nx) {
+                    reportError("ALGLIBInterface", "solve", "The initial guess dimension does not match the problem dimension.");
+                    return false;
+                }
+            }
+
+
+            alglib::real_1d_array alglibX0, alglibStart, alglibSolution, alglibScaling, alglibLowerBound, alglibUpperBound;
+            alglibX0.attach_to_ptr(m_pimpl->x0.size(), m_pimpl->x0.data());
+            if (m_pimpl->alglibScaling.length() != nx)
+                m_pimpl->alglibScaling.setlength(nx);
+            Eigen::Map<Eigen::VectorXd> scaling_Map(m_pimpl->alglibScaling.getcontent(), m_pimpl->alglibScaling.length());
+            scaling_Map.setConstant(1.0); //no scaling
+
+
+            // Create optimizer object, choose AUL algorithm and tune its settings:
+            // * rho=1000       penalty coefficient
+            // * outerits=5     number of outer iterations to tune Lagrange coefficients
+            // * epsx=0.000001  stopping condition for inner iterations
+            // * s=[1,1]        all variables have unit scale
+            // * exact low-rank preconditioner is used, updated after each 10 iterations
+            //
+            if ((m_pimpl->exitCode >= 0) && (m_pimpl->solution.size() == nx)){
+                alglibStart.attach_to_ptr(m_pimpl->solution.size(), m_pimpl->solution.data());
+                alglib::minnlcrestartfrom(m_pimpl->alglibState, alglibStart);
+            } else {
+                alglib::minnlccreate(nx, alglibX0, m_pimpl->alglibState);
+            }
+
+            alglib::minnlcsetalgoaul(m_pimpl->alglibState, m_pimpl->rho, m_pimpl->outerits);
+            alglib::minnlcsetcond(m_pimpl->alglibState, m_pimpl->epsg, m_pimpl->epsf, m_pimpl->epsx, m_pimpl->maxits);
+            alglib::minnlcsetscale(m_pimpl->alglibState, m_pimpl->alglibScaling);
+            alglib::minnlcsetprecinexact(m_pimpl->alglibState);
+
+            alglibLowerBound.attach_to_ptr(m_pimpl->variablesLowerBounds.size(), m_pimpl->variablesLowerBounds.data());
+            alglibUpperBound.attach_to_ptr(m_pimpl->variablesUpperBounds.size(), m_pimpl->variablesUpperBounds.data());
+
+            alglib::minnlcsetbc(m_pimpl->alglibState, alglibLowerBound, alglibUpperBound);
+            alglib::minnlcsetnlc(m_pimpl->alglibState, m_pimpl->nlpData->expandedToOriginalEqualities.size(), m_pimpl->nlpData->expandedToOriginalInequalities.size());
+
+            alglib::minnlcoptimize(m_pimpl->alglibState, ALGLIB_NLP, nullptr, m_pimpl->nlpData);
+
+            alglibSolution.attach_to_ptr(m_pimpl->solution.size(), m_pimpl->solution.data());
+            alglib::minnlcresultsbuf(m_pimpl->alglibState, alglibSolution, m_pimpl->alglibReport);
+            m_pimpl->exitCode = m_pimpl->alglibReport.terminationtype;
+
+            if (m_pimpl->exitCode < 0) {
+                std::ostringstream errorMsg;
+                errorMsg << "Optimization problem failed because ";
+                switch(m_pimpl->exitCode){
+                    case -3:
+                        errorMsg << "constraints are inconsistent.";
+                        break;
+
+                    case -5:
+                        errorMsg << "inappropriate solver was used.";
+                        break;
+
+                    case -7:
+                        errorMsg << "derivative correctness check failed.";
+                        break;
+
+                    case -8:
+                        errorMsg << "optimizer detected NAN/INF values either in the function itself, or in its Jacobian.";
+                        break;
+
+                    default:
+                        errorMsg << "of an unknown error.";
+                }
+                reportError("ALGLIBInterface", "solve", errorMsg.str().c_str());
+                return false;
+            }
+
+            return true;
+        }
+
+        bool AlglibInterface::getPrimalVariables(VectorDynSize &primalVariables)
+        {
+            if (m_pimpl->exitCode < 0){
+                reportError("ALGLIBInterface", "getPrimalVariables", "Previous run was not successfull. Cannot retrieve primal variables.");
+                return false;
+            }
+            primalVariables = m_pimpl->solution;
+            return true;
+        }
+
+        bool AlglibInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
+        {
+            reportError("ALGLIBInterface", "getDualVariables", "ALGLIB does not give information about the dual variables.");
+            return false;
+        }
+
+        bool AlglibInterface::getOptimalCost(double &optimalCost)
+        {
+            if (m_pimpl->exitCode < 0){
+                reportError("ALGLIBInterface", "getOptimalCost", "Previous run was not successfull. Cannot retrieve optimal cost.");
+                return false;
+            }
+
+            if (!(m_pimpl->nlpData->problem->setVariables(m_pimpl->solution))){
+                reportError("ALGLIBInterface", "getOptimalCost", "Error while setting the optimal variables to the problem.");
+                return false;
+            }
+
+            double cost = 0;
+            if (!(m_pimpl->nlpData->problem->evaluateCostFunction(cost))){
+                reportError("ALGLIBInterface", "getOptimalCost", "Error while evaluating the cost function.");
+                return false;
+            }
+            optimalCost = cost;
+            return true;
+        }
+
+        bool AlglibInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
+        {
+            if (m_pimpl->exitCode < 0){
+                reportError("ALGLIBInterface", "getOptimalConstraintsValues", "Previous run was not successfull. Cannot retrieve optimal constraints value.");
+                return false;
+            }
+
+            if (!(m_pimpl->nlpData->problem->setVariables(m_pimpl->solution))){
+                reportError("ALGLIBInterface", "getOptimalConstraintsValues", "Error while setting the optimal variables to the problem.");
+                return false;
+            }
+
+            if (!(m_pimpl->nlpData->problem->evaluateConstraints(m_pimpl->nlpData->variablesBuffer))){
+                reportError("ALGLIBInterface", "getOptimalConstraintsValues", "Error while evaluating the constraints function.");
+                return false;
+            }
+            constraintsValues = m_pimpl->nlpData->variablesBuffer;
+            return true;
+        }
+
+        double AlglibInterface::minusInfinity()
+        {
+            return alglib::fp_neginf;
+        }
+
+        double AlglibInterface::plusInfinity()
+        {
+            return alglib::fp_posinf;
+        }
+
+        bool AlglibInterface::setRHO(double rho)
+        {
+            if (rho <= 0){
+                reportError("ALGLIBInterface", "setRHO", "The rho parameter is supossed to be positive;");
+            }
+            m_pimpl->rho = rho;
+            return true;
+        }
+
+        bool AlglibInterface::setOuterIterations(unsigned int outerIterations)
+        {
+            m_pimpl->outerits = outerIterations;
+            return true;
+        }
+
+        bool AlglibInterface::setStoppingConditions(double epsG, double epsF, double epsX)
+        {
+            if (epsG < 0){
+                reportError("ALGLIBInterface", "setStoppingConditions", "The epsG parameter is supposed to be non negative");
+                return false;
+            }
+
+            if (epsF < 0){
+                reportError("ALGLIBInterface", "setStoppingConditions", "The epsF parameter is supposed to be non negative");
+                return false;
+            }
+
+            if (epsX < 0){
+                reportError("ALGLIBInterface", "setStoppingConditions", "The epsX parameter is supposed to be non negative");
+                return false;
+            }
+
+            m_pimpl->epsg = epsG;
+            m_pimpl->epsf = epsF;
+            m_pimpl->epsx = epsX;
+
+            return true;
+        }
+
+        bool AlglibInterface::setMaximumIterations(unsigned int maxIter)
+        {
+            m_pimpl->maxits = maxIter;
+            return true;
+        }
+
+    }
+
+}
+
diff --git a/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp b/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp
new file mode 100644
index 00000000000..f74be18ef5d
--- /dev/null
+++ b/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/AlglibInterface.h>
+#include <iDynTree/Core/Utils.h>
+
+using namespace iDynTree::optimization;
+
+AlglibInterface::AlglibInterface()
+{
+
+}
+
+AlglibInterface::~AlglibInterface()
+{
+
+}
+
+bool AlglibInterface::isAvailable() const
+{
+    return false;
+}
+
+bool AlglibInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+{
+    reportError("AlglibInterface", "setProblem", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool AlglibInterface::solve()
+{
+    reportError("AlglibInterface", "solve", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool AlglibInterface::getPrimalVariables(VectorDynSize &primalVariables)
+{
+    reportError("AlglibInterface", "getPrimalVariables", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool AlglibInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
+{
+    reportError("AlglibInterface", "getDualVariables", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool AlglibInterface::getOptimalCost(double &optimalCost)
+{
+    reportError("AlglibInterface", "getOptimalCost", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+bool AlglibInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
+{
+    reportError("AlglibInterface", "getOptimalConstraintsValues", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return false;
+}
+
+double AlglibInterface::minusInfinity()
+{
+    reportError("AlglibInterface", "minusInfinity", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return Optimizer::minusInfinity();
+}
+
+double AlglibInterface::plusInfinity()
+{
+    reportError("AlglibInterface", "plusInfinity", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    return Optimizer::plusInfinity();
+}
+
+
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 904a34e0414..80da8a227d3 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -1007,6 +1007,14 @@ namespace iDynTree {
                         }
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
+                        for (unsigned int i = 0; i < nc; ++i) {
+                            if ((m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i)) <= m_minusInfinity) &&
+                                    (m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i)) >= m_plusInfinity)) { //unbounded case
+                                m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i)) = m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i))/2; //avoid unboundness
+                                m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i)) = m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i))/2; //avoid unboundness
+                            }
+                        }
+
                         //Saving the jacobian structure due to the constraints
                         addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         constraintIndex += nc;
@@ -1059,6 +1067,14 @@ namespace iDynTree {
                         }
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
+                        for (unsigned int i = 0; i < nc; ++i) {
+                            if ((m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i)) <= m_minusInfinity) &&
+                                    (m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i)) >= m_plusInfinity)) { //unbounded case
+                                m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i)) = m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i))/2; //avoid unboundness
+                                m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i)) = m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i))/2; //avoid unboundness
+                            }
+                        }
+
                         //Saving the jacobian structure due to the constraints
                         addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
@@ -1113,6 +1129,14 @@ namespace iDynTree {
                         }
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nc)) = toEigen(m_constraintsBuffer);
 
+                        for (unsigned int i = 0; i < nc; ++i) {
+                            if ((m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i)) <= m_minusInfinity) &&
+                                    (m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i)) >= m_plusInfinity)) { //unbounded case
+                                m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i)) = m_constraintsLowerBound(static_cast<unsigned int>(constraintIndex + i))/2; //avoid unboundness
+                                m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i)) = m_constraintsUpperBound(static_cast<unsigned int>(constraintIndex + i))/2; //avoid unboundness
+                            }
+                        }
+
                         addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         constraintIndex += nc;
diff --git a/src/optimalcontrol/src/WorhpInterface.cpp b/src/optimalcontrol/src/WorhpInterface.cpp
new file mode 100644
index 00000000000..3697254ceb9
--- /dev/null
+++ b/src/optimalcontrol/src/WorhpInterface.cpp
@@ -0,0 +1,830 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/WorhpInterface.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Core/Triplets.h>
+#include <worhp/worhp.h>
+#include <algorithm>
+#include <unordered_map>
+
+using namespace iDynTree::optimization;
+
+typedef struct {
+    OptVar opt;
+    Workspace wsp;
+    Params par;
+    Control cnt;
+    int status;
+} WorhpVariables;
+
+class MatrixElement {
+
+    bool prioritySign() const{
+        int thisIndexDifference = static_cast<int>(row) - static_cast<int>(col);
+        return (thisIndexDifference > 0) - (thisIndexDifference < 0); // +1 is the lower triangular part, 0 the diagonal, -1 the upper triangular part
+    }
+
+public:
+    unsigned int row;
+    unsigned int col;
+
+    bool operator < (const MatrixElement& other) const {
+        return this->col < other.col
+                || (this->col == other.col && this->row < other.row);
+    }
+
+    bool lowerTriangularCompare(const MatrixElement& other) const {
+        return prioritySign() > other.prioritySign()
+                || (prioritySign() == other.prioritySign() && this->operator<(other));
+    }
+};
+
+class WorhpInterface::WorhpInterfaceImplementation {
+public:
+    WorhpVariables worhp;
+    std::vector<size_t> constraintsJacNNZRows, constraintsJacNNZCols, hessianNNZRows, hessianNNZCols;
+    size_t hessianLowerTriangularNonZeros;
+    iDynTree::VectorDynSize initialPoint;
+    iDynTree::VectorDynSize variablesLowerBounds, variablesUpperBounds, constraintsLowerBounds, constraintsUpperBounds;
+    iDynTree::VectorDynSize variablesBuffer, constraintsEvaluationBuffer, costGradientBuffer, constraintsMultipliersBuffer;
+    iDynTree::MatrixDynSize constraintsJacobianBuffer, costHessianBuffer, constraintsHessianBuffer;
+    std::vector<MatrixElement> orderedJacobianNonZeros, orderedHessianNonZeros;
+    unsigned int previousNumberOfVariables, previousNumberOfConstraints, previousJacobianNonZeros, previousHessianNonZeros;
+    std::unordered_map<std::string, bool> boolParamsBackup;
+    std::unordered_map<std::string, int> intParamsBackup;
+    std::unordered_map<std::string, double> doubleParamsBackup;
+    bool sparseJacobian, sparseHessian;
+    bool previouslySolved;
+    bool firstTime;
+
+
+    void resizeBuffers(unsigned int numberOfVariables, unsigned int numberOfConstraints) {
+        variablesBuffer.resize(numberOfVariables);
+        constraintsEvaluationBuffer.resize(numberOfConstraints);
+        costGradientBuffer.resize(numberOfVariables);
+        constraintsJacobianBuffer.resize(numberOfConstraints,numberOfVariables);
+        constraintsJacobianBuffer.zero();
+        costHessianBuffer.resize(numberOfVariables, numberOfVariables);
+        costHessianBuffer.zero();
+        constraintsHessianBuffer.resize(numberOfVariables, numberOfVariables);
+        constraintsHessianBuffer.zero();
+        constraintsMultipliersBuffer.resize(numberOfConstraints);
+        initialPoint.resize(numberOfVariables);
+
+    }
+
+    bool possibleReOptimize(unsigned int newNumberOfVariables, unsigned int newNumberOfconstraints,
+                            unsigned int newJacobianNonZeros, unsigned int newHessianNonZeros){
+        bool sameVariables = (previousNumberOfVariables == newNumberOfVariables);
+        bool sameConstraints = (previousNumberOfConstraints == newNumberOfconstraints);
+        bool sameNNZJac = (previousJacobianNonZeros == newJacobianNonZeros);
+        bool sameNNZHes = (previousHessianNonZeros == newHessianNonZeros);
+        return previouslySolved && sameVariables && sameConstraints && sameNNZJac && sameNNZHes;
+    }
+
+    bool updateVariables(std::shared_ptr<iDynTree::optimization::OptimizationProblem> problem) {
+        if (worhp.opt.newX) {
+            Eigen::Map<const Eigen::VectorXd> variablesMap(worhp.opt.X, worhp.opt.n);
+            iDynTree::toEigen(variablesBuffer) = variablesMap;
+            if (!problem->setVariables(variablesBuffer)) {
+                reportError("WorhpInterface", "solve", "Failed to update variables.");
+                previouslySolved = false;
+                return false;
+            }
+        }
+        return true;
+    }
+
+};
+
+
+WorhpInterface::WorhpInterface()
+    : m_pimpl(new WorhpInterfaceImplementation)
+{
+    WorhpPreInit(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+    CHECK_WORHP_VERSION;
+    m_pimpl->previouslySolved = false;
+    m_pimpl->firstTime = true;
+    /*
+    * Parameter initialisation routine
+    */
+    InitParams(&(m_pimpl->worhp.status), &(m_pimpl->worhp.par));
+}
+
+WorhpInterface::~WorhpInterface()
+{
+    if (m_pimpl) {
+        WorhpFree(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+        delete m_pimpl;
+        m_pimpl = nullptr;
+    }
+}
+
+bool WorhpInterface::isAvailable() const
+{
+    return true;
+}
+
+bool WorhpInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+{
+    if (!problem){
+        reportError("WorhpInterface", "setProblem", "Empty problem pointer.");
+        return false;
+    }
+    m_problem = problem;
+    m_pimpl->previouslySolved = false;
+    return true;
+}
+
+bool WorhpInterface::solve()
+{
+    if (!m_problem) {
+        reportError("WorhpInterface", "solve", "Optimization problem not set.");
+        m_pimpl->previouslySolved = false;
+        return false;
+    }
+
+    if (!m_problem->prepare()){
+        reportError("WorhpInterface", "solve", "Error while preparing the optimization problem.");
+        m_pimpl->previouslySolved = false;
+        return false;
+    }
+
+    unsigned int n = m_problem->numberOfVariables();
+    unsigned int m = m_problem->numberOfConstraints();
+    unsigned int nonZerosJacobian = 0;
+    unsigned int nonZerosHessian = 0;
+
+    if (m_problem->info().hasSparseConstraintJacobian()) {
+        if (!(m_problem->getConstraintsJacobianInfo(m_pimpl->constraintsJacNNZRows,
+                                                    m_pimpl->constraintsJacNNZCols))){
+            reportError("WorhpInterface", "solve", "Error while retrieving constraints jacobian info.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        if (m_pimpl->constraintsJacNNZCols.size() != m_pimpl->constraintsJacNNZRows.size()) {
+            reportError("WorhpInterface", "solve", "The vectors of nonzeros of the constraints jacobian have different sizes.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        nonZerosJacobian = static_cast<unsigned int>(m_pimpl->constraintsJacNNZRows.size());
+
+        m_pimpl->orderedJacobianNonZeros.resize(m_pimpl->constraintsJacNNZRows.size());
+
+        for (size_t i = 0; i < m_pimpl->constraintsJacNNZRows.size(); ++i) {
+            m_pimpl->orderedJacobianNonZeros[i].row = static_cast<unsigned int>(m_pimpl->constraintsJacNNZRows[i]);
+            m_pimpl->orderedJacobianNonZeros[i].col = static_cast<unsigned int>(m_pimpl->constraintsJacNNZCols[i]);
+        }
+
+        std::sort(m_pimpl->orderedJacobianNonZeros.begin(), m_pimpl->orderedJacobianNonZeros.end());
+
+        m_pimpl->sparseJacobian = true;
+
+    } else {
+        nonZerosJacobian = m*n;
+        m_pimpl->sparseJacobian = false;
+    }
+
+    if (!(m_problem->info().hessianIsProvided())) {
+        if (!setWorhpParam("UserHM", false)) {
+            reportError("WorhpInterface", "solve", "Failed to set UserHM option.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+    }
+
+    if (m_problem->info().hasSparseHessian()) {
+        if (!(m_problem->getHessianInfo(m_pimpl->hessianNNZRows,
+                                        m_pimpl->hessianNNZCols))){
+            reportError("WorhpInterface", "solve", "Error while retrieving hessian info.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        if (m_pimpl->hessianNNZRows.size() != m_pimpl->hessianNNZCols.size()) {
+            reportError("WorhpInterface", "solve", "The vectors of nonzeros of the hessian matrix have different sizes.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        m_pimpl->hessianLowerTriangularNonZeros = 0;
+        for (size_t i = 0; i < m_pimpl->hessianNNZRows.size(); ++i) {
+            if (m_pimpl->hessianNNZRows[i] > m_pimpl->hessianNNZCols[i]) {
+                m_pimpl->hessianLowerTriangularNonZeros++;
+            }
+        }
+
+        nonZerosHessian = static_cast<unsigned int>(m_pimpl->hessianLowerTriangularNonZeros) + n; //add the diagonal elements
+
+        m_pimpl->orderedHessianNonZeros.resize(m_pimpl->hessianNNZRows.size());
+
+        for (size_t i = 0; i < m_pimpl->hessianNNZRows.size(); ++i) {
+            m_pimpl->orderedHessianNonZeros[i].row = static_cast<unsigned int>(m_pimpl->hessianNNZRows[i]);
+            m_pimpl->orderedHessianNonZeros[i].col = static_cast<unsigned int>(m_pimpl->hessianNNZCols[i]);
+        }
+
+        std::sort(m_pimpl->orderedHessianNonZeros.begin(), m_pimpl->orderedHessianNonZeros.end(),
+                  [](const MatrixElement &a, const MatrixElement &b) {return a.lowerTriangularCompare(b);});
+
+        m_pimpl->sparseHessian = true;
+
+    } else {
+        nonZerosHessian = n*(n + 1)/2;
+        if (!setWorhpParam("UserHMstructure", 0)) {
+            reportError("WorhpInterface", "solve", "Failed to set UserHMstructure option.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        if (!setWorhpParam("FidifHM", true)) {
+            reportError("WorhpInterface", "solve", "Failed to set BFGSmethod option.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        m_pimpl->sparseHessian = false;
+    }
+
+    if (m_pimpl->possibleReOptimize(n, m, nonZerosJacobian, nonZerosHessian)) {
+
+        WorhpRestart (&(m_pimpl->worhp.opt), &(m_pimpl->worhp.wsp), &(m_pimpl->worhp.par), &(m_pimpl->worhp.cnt));
+
+    } else {
+
+        if (m_pimpl->firstTime) {
+            m_pimpl->firstTime = false;
+        } else {
+            WorhpFree(&(m_pimpl->worhp.opt), &(m_pimpl->worhp.wsp), &(m_pimpl->worhp.par), &(m_pimpl->worhp.cnt));
+
+            WorhpPreInit(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+
+            InitParams(&(m_pimpl->worhp.status), &(m_pimpl->worhp.par));
+
+            for (auto boolParam = m_pimpl->boolParamsBackup.begin(); boolParam != m_pimpl->boolParamsBackup.end(); ++boolParam) {
+                WorhpSetBoolParam(&(m_pimpl->worhp.par), boolParam->first.c_str() , boolParam->second);
+            }
+
+            for (auto intParam = m_pimpl->intParamsBackup.begin(); intParam != m_pimpl->intParamsBackup.end(); ++intParam) {
+                WorhpSetIntParam(&(m_pimpl->worhp.par), intParam->first.c_str() , intParam->second);
+            }
+
+            for (auto doubleParam = m_pimpl->doubleParamsBackup.begin(); doubleParam != m_pimpl->doubleParamsBackup.end(); ++doubleParam) {
+                WorhpSetDoubleParam(&(m_pimpl->worhp.par), doubleParam->first.c_str() , doubleParam->second);
+            }
+        }
+
+        m_pimpl->worhp.opt.n = static_cast<int>(n);
+        m_pimpl->worhp.opt.m = static_cast<int>(m);
+
+        m_pimpl->worhp.wsp.DF.nnz = static_cast<int>(n); //Assume dense gradient
+
+        m_pimpl->worhp.wsp.DG.nnz = static_cast<int>(nonZerosJacobian);
+
+        m_pimpl->worhp.wsp.HM.nnz = static_cast<int>(nonZerosHessian);
+
+        m_pimpl->resizeBuffers(n, m);
+
+        // plus full diagonal
+        WorhpInit(&(m_pimpl->worhp.opt), &(m_pimpl->worhp.wsp), &(m_pimpl->worhp.par), &(m_pimpl->worhp.cnt));
+
+        if (m_pimpl->worhp.cnt.status != FirstCall)
+        {
+            reportError("WorhpInterface", "solve", "Failed to initialize WORHP structures.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+
+        if (m_problem->info().costIsLinear()) {
+            m_pimpl->worhp.opt.FType = WORHP_LINEAR;
+        } else if (m_problem->info().costIsQuadratic()) {
+            m_pimpl->worhp.opt.FType = WORHP_QUADRATIC;
+        } else {
+            m_pimpl->worhp.opt.FType = WORHP_NONLINEAR;
+        }
+
+        if (!(m_problem->info().hasNonLinearConstraints())) {
+            Eigen::Map<Eigen::VectorXi> gTypesMap(m_pimpl->worhp.opt.GType, m_pimpl->worhp.opt.m);
+            gTypesMap.setConstant(WORHP_LINEAR);
+        } else {
+            Eigen::Map<Eigen::VectorXi> gTypesMap(m_pimpl->worhp.opt.GType, m_pimpl->worhp.opt.m);
+            gTypesMap.setConstant(WORHP_NONLINEAR);
+        }
+
+
+        Eigen::Map<Eigen::VectorXd> initialBoundsMultipliersMap(m_pimpl->worhp.opt.Lambda, n);
+        initialBoundsMultipliersMap.setZero();
+
+        Eigen::Map<Eigen::VectorXd> initialConstraintsMultipliersMap(m_pimpl->worhp.opt.Mu, m);
+        initialConstraintsMultipliersMap.setZero();
+
+        Eigen::Map<Eigen::VectorXd> initialPointMap(m_pimpl->worhp.opt.X, n);
+        initialPointMap.setZero();
+    }
+
+    if (m_problem->getGuess(m_pimpl->initialPoint)) {
+        if (m_pimpl->initialPoint.size() != n) {
+            reportError("WorhpInterface", "solve", "The specified guess size does not match the number of variables.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+        Eigen::Map<Eigen::VectorXd> initialPointMap(m_pimpl->worhp.opt.X, n);
+        initialPointMap = iDynTree::toEigen(m_pimpl->initialPoint);
+    }
+
+    if (m_problem->getVariablesLowerBound(m_pimpl->variablesLowerBounds)) {
+        if (m_pimpl->variablesLowerBounds.size() != n) {
+            reportError("WorhpInterface", "solve", "The size of the variables lowerbound does not match the number of variables.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+        Eigen::Map<Eigen::VectorXd> variablesLowerBoundMap(m_pimpl->worhp.opt.XL, n);
+        variablesLowerBoundMap = iDynTree::toEigen(m_pimpl->variablesLowerBounds);
+    } else {
+        Eigen::Map<Eigen::VectorXd> variablesLowerBoundMap(m_pimpl->worhp.opt.XL, n);
+        variablesLowerBoundMap.setConstant(-m_pimpl->worhp.par.Infty);
+    }
+
+    if (m_problem->getVariablesUpperBound(m_pimpl->variablesUpperBounds)) {
+        if (m_pimpl->variablesUpperBounds.size() != n) {
+            reportError("WorhpInterface", "solve", "The size of the variables upperbound does not match the number of variables.");
+            m_pimpl->previouslySolved = false;
+            return false;
+        }
+        Eigen::Map<Eigen::VectorXd> variablesUpperBoundMap(m_pimpl->worhp.opt.XU, n);
+        variablesUpperBoundMap = iDynTree::toEigen(m_pimpl->variablesUpperBounds);
+    } else {
+        Eigen::Map<Eigen::VectorXd> variablesUpperBoundMap(m_pimpl->worhp.opt.XU, n);
+        variablesUpperBoundMap.setConstant(m_pimpl->worhp.par.Infty);
+    }
+
+    if (!(m_problem->getConstraintsBounds(m_pimpl->constraintsLowerBounds, m_pimpl->constraintsUpperBounds))) {
+        reportError("WorhpInterface", "solve", "Failed to get constraints bounds.");
+        m_pimpl->previouslySolved = false;
+        return false;
+    }
+
+    if (m_pimpl->constraintsLowerBounds.size() != m_pimpl->constraintsUpperBounds.size()) {
+        reportError("WorhpInterface", "solve", "Constraints bounds have different sizes.");
+        m_pimpl->previouslySolved = false;
+        return false;
+    }
+
+    if (m_pimpl->constraintsLowerBounds.size() != m) {
+        reportError("WorhpInterface", "solve", "Constraints bounds have size different from the number of constraints.");
+        m_pimpl->previouslySolved = false;
+        return false;
+    }
+
+    Eigen::Map<Eigen::VectorXd> constraintsLowerBoundMap(m_pimpl->worhp.opt.GL, m);
+    constraintsLowerBoundMap = iDynTree::toEigen(m_pimpl->constraintsLowerBounds);
+
+    Eigen::Map<Eigen::VectorXd> constraintsUpperBoundMap(m_pimpl->worhp.opt.GU, m);
+    constraintsUpperBoundMap = iDynTree::toEigen(m_pimpl->constraintsUpperBounds);
+
+    /*
+    * Specify matrix structures in CS format, using Fortran indexing,
+    * i.e. 1...N instead of 0...N-1, to describe the matrix structure.
+    */
+    // Define DF as row vector, column index is ommited
+    if (m_pimpl->worhp.wsp.DF.NeedStructure) {
+        for (int i = 0; i < static_cast<int>(n); ++i) {
+            m_pimpl->worhp.wsp.DF.row[i] = i + 1; //dense
+        }
+    }
+    // Define DG as CS-matrix
+    if (m_pimpl->worhp.wsp.DG.NeedStructure) {
+        // only set the nonzero entries in column-major order
+        if (m_pimpl->sparseJacobian) {
+            for (size_t el = 0; el < m_pimpl->orderedJacobianNonZeros.size(); ++el) {
+                m_pimpl->worhp.wsp.DG.row[el] = static_cast<int>(m_pimpl->orderedJacobianNonZeros[el].row) + 1;
+                m_pimpl->worhp.wsp.DG.col[el] = static_cast<int>(m_pimpl->orderedJacobianNonZeros[el].col) + 1;
+            }
+        } else {
+            size_t element = 0;
+            for (unsigned int  col = 0; col < n; ++col) {
+                for (unsigned int row = 0; row < m; ++row) {
+                    m_pimpl->worhp.wsp.DG.row[element] = static_cast<int>(row) + 1;
+                    m_pimpl->worhp.wsp.DG.col[element] = static_cast<int>(col) + 1;
+                    element++;
+                }
+            }
+        }
+    }
+
+    if (m_pimpl->worhp.wsp.HM.NeedStructure) {
+        /*
+        * only set the nonzero entries, with the strictly lower triangle first,
+        * followed by ALL diagonal entries
+        */
+        // strict lower triangle
+        if (m_pimpl->sparseHessian) {
+            for (size_t el = 0; el < m_pimpl->hessianLowerTriangularNonZeros; ++el) {
+                m_pimpl->worhp.wsp.HM.row[el] = static_cast<int>(m_pimpl->orderedHessianNonZeros[el].row) + 1;
+                m_pimpl->worhp.wsp.HM.col[el] = static_cast<int>(m_pimpl->orderedHessianNonZeros[el].col) + 1;
+            }
+
+        } else {
+            size_t element = 0;
+            for (unsigned int  col = 1; col < n; ++col) {
+                for (unsigned int row = 0; row < col; ++row) {
+                    m_pimpl->worhp.wsp.HM.row[element] = static_cast<int>(row) + 1;
+                    m_pimpl->worhp.wsp.HM.col[element] = static_cast<int>(col) + 1;
+                    element++;
+                }
+            }
+        }
+        // diagonal
+        for (int i = 0; i < m_pimpl->worhp.opt.n; i++) {
+            m_pimpl->worhp.wsp.HM.row[m_pimpl->worhp.wsp.HM.nnz - m_pimpl->worhp.opt.n + i] = i + 1;
+            m_pimpl->worhp.wsp.HM.col[m_pimpl->worhp.wsp.HM.nnz - m_pimpl->worhp.opt.n + i] = i + 1;
+        }
+    }
+
+    Eigen::Map<const Eigen::VectorXd> variablesMap(m_pimpl->worhp.opt.X, m_pimpl->worhp.opt.n);
+    iDynTree::toEigen(m_pimpl->variablesBuffer) = variablesMap;
+    if (!m_problem->setVariables(m_pimpl->variablesBuffer)) {
+        reportError("WorhpInterface", "solve", "Failed to update variables.");
+        m_pimpl->previouslySolved = false;
+        return false;
+    }
+
+    /*
+    * WORHP Reverse Communication loop.
+    * In every iteration poll GetUserAction for the requested action, i.e. one
+    * of {callWorhp, iterOutput, evalF, evalG, evalDF, evalDG, evalHM, fidif}.
+    *
+    * Make sure to reset the requested user action afterwards by calling
+    * DoneUserAction, except for 'callWorhp' and 'fidif'.
+    */
+    while (m_pimpl->worhp.cnt.status < TerminateSuccess && m_pimpl->worhp.cnt.status > TerminateError) {
+        /*
+        * WORHP's main routine.
+        * Do not manually reset callWorhp, this is only done by the FD routines.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, callWorhp)) {
+            Worhp(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+            // No DoneUserAction!
+        }
+
+        /*
+        * Show iteration output.
+        * The call to IterationOutput() may be replaced by user-defined code.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, iterOutput)) {
+            IterationOutput(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+            DoneUserAction(&m_pimpl->worhp.cnt, iterOutput);
+        }
+
+        /*
+        * Evaluate the objective function.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, evalF)) {
+
+            if (!(m_pimpl->updateVariables(m_problem))) {
+                    return false;
+            }
+
+            double costValue = 0;
+            if (!m_problem->evaluateCostFunction(costValue)) {
+                reportError("WorhpInterface", "solve", "Failed to evaluate cost.");
+                m_pimpl->previouslySolved = false;
+                return false;
+            }
+
+            m_pimpl->worhp.opt.F = m_pimpl->worhp.wsp.ScaleObj * costValue;
+
+            DoneUserAction(&m_pimpl->worhp.cnt, evalF);
+        }
+
+        /*
+        * Evaluate the constraints.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, evalG)) {
+
+            if (!(m_pimpl->updateVariables(m_problem))) {
+                    return false;
+            }
+
+            if (!m_problem->evaluateConstraints(m_pimpl->constraintsEvaluationBuffer)) {
+                reportError("WorhpInterface", "solve", "Failed to evaluate constraints.");
+                m_pimpl->previouslySolved = false;
+                return false;
+            }
+            assert(m_pimpl->constraintsEvaluationBuffer.size() == m);
+            Eigen::Map<Eigen::VectorXd> constraintsMap(m_pimpl->worhp.opt.G, m);
+            constraintsMap = iDynTree::toEigen(m_pimpl->constraintsEvaluationBuffer);
+
+            DoneUserAction(&m_pimpl->worhp.cnt, evalG);
+        }
+
+        /*
+        * Evaluate the gradient of the objective function.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, evalDF)) {
+
+            if (!(m_pimpl->updateVariables(m_problem))) {
+                    return false;
+            }
+
+            if (!m_problem->evaluateCostGradient(m_pimpl->costGradientBuffer)) {
+                reportError("WorhpInterface", "solve", "Failed to evaluate cost gradient.");
+                m_pimpl->previouslySolved = false;
+                return false;
+            }
+            assert(m_pimpl->costGradientBuffer.size() == n);
+            Eigen::Map<Eigen::VectorXd> costGradientMap(m_pimpl->worhp.wsp.DF.val, n); //dense vector
+            costGradientMap = m_pimpl->worhp.wsp.ScaleObj *  iDynTree::toEigen(m_pimpl->costGradientBuffer);
+            DoneUserAction(&m_pimpl->worhp.cnt, evalDF);
+        }
+
+        /*
+        * Evaluate the Jacobian of the constraints.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, evalDG)) {
+
+            if (!(m_pimpl->updateVariables(m_problem))) {
+                    return false;
+            }
+
+            if (!m_problem->evaluateConstraintsJacobian(m_pimpl->constraintsJacobianBuffer)) {
+                reportError("WorhpInterface", "solve", "Failed to evaluate constraints jacobian.");
+                m_pimpl->previouslySolved = false;
+                return false;
+            }
+
+            if (m_pimpl->sparseJacobian) {
+                for (size_t i = 0; i < m_pimpl->orderedJacobianNonZeros.size(); ++i) {
+                    m_pimpl->worhp.wsp.DG.val[i] =
+                            m_pimpl->constraintsJacobianBuffer(
+                                m_pimpl->orderedJacobianNonZeros[i].row,
+                                m_pimpl->orderedJacobianNonZeros[i].col);
+                }
+            } else {
+                size_t element = 0;
+                for (unsigned int  col = 0; col < n; ++col) {
+                    for (unsigned int row = 0; row < m; ++row) {
+                        m_pimpl->worhp.wsp.DG.val[element] = m_pimpl->constraintsJacobianBuffer(row, col);
+                        element++;
+                    }
+                }
+            }
+
+            DoneUserAction(&m_pimpl->worhp.cnt, evalDG);
+        }
+
+        /*
+        * Evaluate the Hessian matrix of the Lagrange function (L = f + mu*g)
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, evalHM)) {
+
+            if (!(m_pimpl->updateVariables(m_problem))) {
+                    return false;
+            }
+
+            if (!m_problem->evaluateCostHessian(m_pimpl->costHessianBuffer)) {
+                reportError("WorhpInterface", "solve", "Failed to get cost hessian.");
+                m_pimpl->previouslySolved = false;
+                return false;
+            }
+
+            Eigen::Map<Eigen::VectorXd> constraintsMultipliersMap(m_pimpl->worhp.opt.Mu, m);
+            iDynTree::toEigen(m_pimpl->constraintsMultipliersBuffer) = constraintsMultipliersMap;
+
+            if (!m_problem->evaluateConstraintsHessian(m_pimpl->constraintsMultipliersBuffer, m_pimpl->constraintsHessianBuffer)) {
+                reportError("WorhpInterface", "solve", "Failed to get constraints hessian.");
+                m_pimpl->previouslySolved = false;
+                return false;
+            }
+
+            if (m_pimpl->sparseHessian) {
+                for (size_t i = 0; i < m_pimpl->hessianLowerTriangularNonZeros; ++i) {
+                    unsigned int row = m_pimpl->orderedJacobianNonZeros[i].row;
+                    unsigned int col = m_pimpl->orderedJacobianNonZeros[i].col;
+                    m_pimpl->worhp.wsp.HM.val[i] =
+                            (m_pimpl->worhp.wsp.ScaleObj * m_pimpl->costHessianBuffer(row, col)) +
+                            m_pimpl->constraintsHessianBuffer(row,col);
+                }
+            } else {
+                size_t element = 0;
+                for (unsigned int  col = 1; col < n; ++col) {
+                    for (unsigned int row = 0; row < col; ++row) {
+                        m_pimpl->worhp.wsp.HM.val[element] =
+                                (m_pimpl->worhp.wsp.ScaleObj * m_pimpl->costHessianBuffer(row, col)) +
+                                m_pimpl->constraintsHessianBuffer(row,col);
+                        element++;
+                    }
+                }
+            }
+            // diagonal
+            for (unsigned int i = 0; i < n; i++) {
+                m_pimpl->worhp.wsp.HM.val[i] =
+                        (m_pimpl->worhp.wsp.ScaleObj * m_pimpl->costHessianBuffer(i, i)) +
+                        m_pimpl->constraintsHessianBuffer(i,i);
+            }
+
+            DoneUserAction(&m_pimpl->worhp.cnt, evalHM);
+        }
+
+        /*
+        * Use finite differences with RC to determine derivatives
+        * Do not reset fidif, this is done by the FD routine.
+        */
+        if (GetUserAction(&m_pimpl->worhp.cnt, fidif)) {
+            WorhpFidif(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+            // No DoneUserAction!
+        }
+    }
+
+    if (m_pimpl->worhp.cnt.status <= TerminateError) {
+        reportError("WorhpInterface", "solve", "Failed to solve the problem. Here additional infos:");
+        m_pimpl->previouslySolved = false;
+        StatusMsg(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+        return false;
+    }
+    m_pimpl->previouslySolved = true;
+    m_pimpl->previousNumberOfVariables = static_cast<unsigned int>(m_pimpl->worhp.opt.n);
+    m_pimpl->previousNumberOfConstraints = static_cast<unsigned int>(m_pimpl->worhp.opt.m);
+    m_pimpl->previousJacobianNonZeros = static_cast<unsigned int>(m_pimpl->worhp.wsp.DG.nnz);
+    m_pimpl->previousHessianNonZeros = static_cast<unsigned int>(m_pimpl->worhp.wsp.HM.nnz);
+
+
+    return true;
+}
+
+bool WorhpInterface::getPrimalVariables(iDynTree::VectorDynSize &primalVariables)
+{
+    if (!(m_pimpl->previouslySolved)) {
+        reportError("WorhpInterface", "getPrimalVariables", "The solve method was not called or it returned false.");
+        return false;
+    }
+
+    Eigen::Map<const Eigen::VectorXd> variablesMap(m_pimpl->worhp.opt.X, m_pimpl->worhp.opt.n);
+    iDynTree::toEigen(m_pimpl->variablesBuffer) = variablesMap;
+
+    primalVariables = m_pimpl->variablesBuffer;
+
+    return true;
+}
+
+bool WorhpInterface::getDualVariables(iDynTree::VectorDynSize &constraintsMultipliers, iDynTree::VectorDynSize &lowerBoundsMultipliers, iDynTree::VectorDynSize &upperBoundsMultipliers)
+{
+    if (!(m_pimpl->previouslySolved)) {
+        reportError("WorhpInterface", "getDualVariables", "The solve method was not called or it returned false.");
+        return false;
+    }
+
+    constraintsMultipliers.resize(static_cast<unsigned int>(m_pimpl->previousNumberOfConstraints));
+    lowerBoundsMultipliers.resize(static_cast<unsigned int>(m_pimpl->previousNumberOfVariables));
+    upperBoundsMultipliers.resize(static_cast<unsigned int>(m_pimpl->previousNumberOfVariables));
+
+    Eigen::Map<const Eigen::VectorXd> constraintsMultipliersMap(m_pimpl->worhp.opt.Mu, m_pimpl->worhp.opt.m);
+    iDynTree::toEigen(constraintsMultipliers) = constraintsMultipliersMap;
+
+    if (m_pimpl->variablesLowerBounds.size() == 0) {
+        lowerBoundsMultipliers.zero();
+
+        if (m_pimpl->variablesUpperBounds.size() == 0) {
+            upperBoundsMultipliers.zero();
+        }
+        else {
+            Eigen::Map<const Eigen::VectorXd> variablesMultipliersMap(m_pimpl->worhp.opt.Lambda, m_pimpl->worhp.opt.n);
+            iDynTree::toEigen(upperBoundsMultipliers) = variablesMultipliersMap;
+        }
+    } else if (m_pimpl->variablesUpperBounds.size() == 0) {
+        upperBoundsMultipliers.zero();
+        Eigen::Map<const Eigen::VectorXd> variablesMultipliersMap(m_pimpl->worhp.opt.Lambda, m_pimpl->worhp.opt.n);
+        iDynTree::toEigen(lowerBoundsMultipliers) = variablesMultipliersMap;
+    } else {
+        double lowerBoundDistance, upperBoundDistance;
+
+        for (unsigned int i = 0;  i < lowerBoundsMultipliers.size(); ++i) {
+            lowerBoundDistance = std::fabs(m_pimpl->worhp.opt.X[i] - m_pimpl->variablesLowerBounds(i));
+            upperBoundDistance = std::fabs(m_pimpl->worhp.opt.X[i] - m_pimpl->variablesUpperBounds(i));
+
+            if (lowerBoundDistance < upperBoundDistance) { //Lower bounds are active
+                lowerBoundsMultipliers(i) = m_pimpl->worhp.opt.Lambda[i];
+                upperBoundsMultipliers(i) = 0.0;
+            } else {
+                lowerBoundsMultipliers(i) = 0.0;
+                upperBoundsMultipliers(i) = m_pimpl->worhp.opt.Lambda[i];
+            }
+        }
+    }
+
+    return true;
+}
+
+bool WorhpInterface::getOptimalCost(double &optimalCost)
+{
+    if (!(m_pimpl->previouslySolved)) {
+        reportError("WorhpInterface", "getOptimalCost", "The solve method was not called or it returned false.");
+        return false;
+    }
+
+    if (!iDynTree::checkDoublesAreEqual(m_pimpl->worhp.wsp.ScaleObj, 0.0)) {
+        optimalCost = m_pimpl->worhp.opt.F / m_pimpl->worhp.wsp.ScaleObj;
+    } else {
+        optimalCost = m_pimpl->worhp.opt.F;
+    }
+
+    return true;
+}
+
+bool WorhpInterface::getOptimalConstraintsValues(iDynTree::VectorDynSize &constraintsValues)
+{
+    if (!(m_pimpl->previouslySolved)) {
+        reportError("WorhpInterface", "getOptimalConstraintsValues", "The solve method was not called or it returned false.");
+        return false;
+    }
+
+    Eigen::Map<const Eigen::VectorXd> constraintsMap(m_pimpl->worhp.opt.G, m_pimpl->worhp.opt.m);
+    iDynTree::toEigen(m_pimpl->constraintsEvaluationBuffer) = constraintsMap;
+
+    constraintsValues = m_pimpl->constraintsEvaluationBuffer;
+
+    return true;
+}
+
+double WorhpInterface::minusInfinity()
+{
+    return -m_pimpl->worhp.par.Infty;
+}
+
+double WorhpInterface::plusInfinity()
+{
+    return m_pimpl->worhp.par.Infty;
+}
+
+bool WorhpInterface::setWorhpParam(const std::string &paramName, bool value)
+{
+    if (!WorhpSetBoolParam(&(m_pimpl->worhp.par) ,paramName.c_str() , value)) {
+        reportError("WorhpInterface", "setWorhpParam", ("Failed to set parameter " + paramName + ". Either it was not found or the expected input value has wrong type.").c_str());
+        return false;
+    }
+    m_pimpl->boolParamsBackup[paramName] = value;
+    return true;
+}
+
+bool WorhpInterface::setWorhpParam(const std::string &paramName, double value)
+{
+    if (!WorhpSetDoubleParam(&(m_pimpl->worhp.par) ,paramName.c_str() , value)) {
+        reportError("WorhpInterface", "setWorhpParam", ("Failed to set parameter " + paramName + ". Either it was not found or the expected input value has wrong type.").c_str());
+        return false;
+    }
+    m_pimpl->doubleParamsBackup[paramName] = value;
+    return true;
+}
+
+bool WorhpInterface::setWorhpParam(const std::string &paramName, int value)
+{
+    if (!WorhpSetIntParam(&(m_pimpl->worhp.par) ,paramName.c_str() , value)) {
+        reportError("WorhpInterface", "setWorhpParam", ("Failed to set parameter " + paramName + ". Either it was not found or the expected input value has wrong type.").c_str());
+        return false;
+    }
+    m_pimpl->intParamsBackup[paramName] = value;
+    return true;
+}
+
+bool WorhpInterface::getWorhpParam(const std::string &paramName, bool &value)
+{
+    if (!WorhpGetBoolParam(&(m_pimpl->worhp.par) ,paramName.c_str() , &value)) {
+        reportError("WorhpInterface", "getWorhpParam", ("Failed to get parameter " + paramName + ". Either it was not found or the expected output value has wrong type.").c_str());
+        return false;
+    }
+    return true;
+}
+
+bool WorhpInterface::getWorhpParam(const std::string &paramName, double &value)
+{
+    if (!WorhpGetDoubleParam(&(m_pimpl->worhp.par) ,paramName.c_str() , &value)) {
+        reportError("WorhpInterface", "getWorhpParam", ("Failed to get parameter " + paramName + ". Either it was not found or the expected output value has wrong type.").c_str());
+        return false;
+    }
+    return true;
+}
+
+bool WorhpInterface::getWorhpParam(const std::string &paramName, int &value)
+{
+    if (!WorhpGetIntParam(&(m_pimpl->worhp.par) ,paramName.c_str() , &value)) {
+        reportError("WorhpInterface", "getWorhpParam", ("Failed to get parameter " + paramName + ". Either it was not found or the expected output value has wrong type.").c_str());
+        return false;
+    }
+    return true;
+}
diff --git a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
new file mode 100644
index 00000000000..782f6b71344
--- /dev/null
+++ b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/Optimizers/WorhpInterface.h>
+#include <iDynTree/Core/Utils.h>
+
+using namespace iDynTree::optimization;
+
+WorhpInterface::WorhpInterface()
+{
+
+}
+
+WorhpInterface::~WorhpInterface()
+{
+
+}
+
+bool WorhpInterface::isAvailable() const
+{
+    return false;
+}
+
+bool WorhpInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
+{
+    reportError("WorhpInterface", "setProblem", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::solve()
+{
+    reportError("WorhpInterface", "solve", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getPrimalVariables(VectorDynSize &primalVariables)
+{
+    reportError("WorhpInterface", "getPrimalVariables", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
+{
+    reportError("WorhpInterface", "getDualVariables", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getOptimalCost(double &optimalCost)
+{
+    reportError("WorhpInterface", "getOptimalCost", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
+{
+    reportError("WorhpInterface", "getOptimalConstraintsValues", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+double WorhpInterface::minusInfinity()
+{
+    reportError("WorhpInterface", "minusInfinity", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return Optimizer::minusInfinity();
+}
+
+double WorhpInterface::plusInfinity()
+{
+    reportError("WorhpInterface", "plusInfinity", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return Optimizer::plusInfinity();
+}
+
+bool WorhpInterface::setWorhpParam(const std::string &paramName, bool value)
+{
+    reportError("WorhpInterface", "setWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::setWorhpParam(const std::string &paramName, double value)
+{
+    reportError("WorhpInterface", "setWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::setWorhpParam(const std::string &paramName, int value)
+{
+    reportError("WorhpInterface", "setWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getWorhpParam(const std::string &paramName, bool &value)
+{
+    reportError("WorhpInterface", "getWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getWorhpParam(const std::string &paramName, double &value)
+{
+    reportError("WorhpInterface", "getWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
+bool WorhpInterface::getWorhpParam(const std::string &paramName, int &value)
+{
+    reportError("WorhpInterface", "getWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+    return false;
+}
+
diff --git a/src/optimalcontrol/tests/AlglibInterfaceTest.cpp b/src/optimalcontrol/tests/AlglibInterfaceTest.cpp
new file mode 100644
index 00000000000..a027afbc4ea
--- /dev/null
+++ b/src/optimalcontrol/tests/AlglibInterfaceTest.cpp
@@ -0,0 +1,214 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+#include <iDynTree/OptimizationProblem.h>
+#include <iDynTree/Optimizers/AlglibInterface.h>
+#include <iDynTree/Core/TestUtils.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <vector>
+#include <memory>
+
+class TestProblem : public iDynTree::optimization::OptimizationProblem {
+    double m_a1, m_b1, m_c1;
+    double m_a2, m_b2, m_c2;
+    double m_plusInfinity, m_minusInfinity;
+    iDynTree::VectorDynSize m_variables, m_expectedVariables;
+
+public:
+    TestProblem()
+    :m_plusInfinity(1e19)
+    ,m_minusInfinity(-1e19)
+    {}
+
+    virtual ~TestProblem() override {}
+
+    virtual bool prepare() override {
+        m_a1 = iDynTree::getRandomDouble(0.01, 10.0);
+        m_b1 = iDynTree::getRandomDouble(-10.0, 10.0);
+        m_c1 = iDynTree::getRandomDouble(-10.0, 10.0);
+        m_a2 = iDynTree::getRandomDouble(0.01, 10.0);
+        m_b2 = iDynTree::getRandomDouble(-10.0, 10.0);
+        m_c2 = iDynTree::getRandomDouble(-10.0, 10.0);
+
+        return true;
+    }
+
+    virtual void reset() override {}
+
+    virtual unsigned int numberOfVariables() override {
+        return 2;
+    }
+
+    virtual unsigned int numberOfConstraints() override {
+        return 2;
+    }
+
+    virtual bool getConstraintsBounds(iDynTree::VectorDynSize& constraintsLowerBounds, iDynTree::VectorDynSize& constraintsUpperBounds) override {
+        constraintsLowerBounds.resize(2);
+        constraintsUpperBounds.resize(2);
+        constraintsLowerBounds(0) = m_c1;
+        constraintsLowerBounds(1) = m_c2;
+        constraintsUpperBounds(0) = m_plusInfinity;
+        constraintsUpperBounds(1) = m_plusInfinity;
+        return true;
+    }
+
+    virtual bool getVariablesUpperBound(iDynTree::VectorDynSize& variablesUpperBound) override {
+        return false;
+    } //return false if not upper bounded
+
+    virtual bool getVariablesLowerBound(iDynTree::VectorDynSize& variablesLowerBound) override {
+        return false;
+    } //return false if not lower bounded
+
+    virtual bool getConstraintsJacobianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
+        nonZeroElementRows.resize(2);
+        nonZeroElementColumns.resize(2);
+
+        nonZeroElementRows[0] = 0;
+        nonZeroElementRows[1] = 1;
+
+        nonZeroElementColumns = nonZeroElementRows;
+
+        return true;
+    }
+
+    virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
+        nonZeroElementRows.resize(2);
+        nonZeroElementColumns.resize(2);
+
+        nonZeroElementRows[0] = 0;
+        nonZeroElementRows[1] = 1;
+
+        nonZeroElementColumns = nonZeroElementRows;
+
+        return true;
+    } //costs and constraints together
+
+    virtual bool setVariables(const iDynTree::VectorDynSize& variables) override {
+        iDynTree::assertTrue(variables.size()==2);
+        m_variables = variables;
+        return true;
+    }
+
+    virtual bool evaluateCostFunction(double& costValue) override {
+        costValue = m_a1 * m_variables(0) * m_variables(0) + m_b1 * m_variables(0) + m_a2 * m_variables(1) * m_variables(1) + m_b2 * m_variables(1);
+        return true;
+    }
+
+    virtual bool evaluateCostGradient(iDynTree::VectorDynSize& gradient) override {
+        gradient.resize(numberOfVariables());
+        gradient(0) = 2 * m_a1 * m_variables(0) + m_b1;
+        gradient(1) = 2 * m_a2 * m_variables(1) + m_b2;
+        return true;
+    }
+
+    virtual bool evaluateCostHessian(iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(2,2);
+        hessian.zero();
+        hessian(0,0) = 2*m_a1;
+        hessian(1,1) = 2*m_a2;
+        return true;
+    }
+
+    virtual bool evaluateConstraints(iDynTree::VectorDynSize& constraints) override {
+        constraints.resize(2);
+        constraints = m_variables;
+        return true;
+    }
+
+    virtual bool evaluateConstraintsJacobian(iDynTree::MatrixDynSize& jacobian) override {
+        jacobian.resize(2,2);
+        jacobian(0,0) = 1.0;
+        jacobian(1,1) = 1.0;
+        return true;
+    } //using dense matrices, but the sparsity pattern is still obtained
+
+    virtual bool evaluateConstraintsHessian(const iDynTree::VectorDynSize& constraintsMultipliers, iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(2,2);
+        hessian.zero();
+        return true;
+    } //using dense matrices, but the sparsity pattern is still obtained
+
+    bool setPlusInfinity(double plusInfinity) {
+        if (plusInfinity < 0)
+            return false;
+
+        m_plusInfinity = plusInfinity;
+        return true;
+    }
+
+    bool setMinusInfinity(double minusInfinity) {
+        if (minusInfinity > 0)
+            return false;
+
+        m_minusInfinity = minusInfinity;
+        return true;
+    }
+
+    double expectedMinimum() {
+        double expected;
+        if (m_c1 < (-m_b1/(2*m_a1)))
+            expected = - m_b1 * m_b1 / (4*m_a1);
+        else
+            expected = m_a1*m_c1*m_c1 + m_b1*m_c1;
+
+        if (m_c2 < (-m_b2/(2*m_a2)))
+            expected += - m_b2 * m_b2 / (4*m_a2);
+        else
+            expected += m_a2*m_c2*m_c2 + m_b2*m_c2;
+
+        return expected;
+    }
+
+    const iDynTree::VectorDynSize& expectedVariables() {
+        m_expectedVariables.resize(2);
+        if (m_c1 < (-m_b1/(2*m_a1)))
+            m_expectedVariables(0) = - m_b1 / (2*m_a1);
+        else
+            m_expectedVariables(0) = m_c1;
+
+        if (m_c2 < (-m_b2/(2*m_a2)))
+            m_expectedVariables(1) = - m_b2 / (2*m_a2);
+        else
+            m_expectedVariables(1) = m_c2;
+
+        return m_expectedVariables;
+    }
+};
+
+int main(){
+    iDynTree::optimization::AlglibInterface alglibSolver;
+    std::shared_ptr<TestProblem> problem(new TestProblem);
+    iDynTree::VectorDynSize guess(2), dummy1, dummy2, dummy3;
+
+    iDynTree::assertTrue(problem->setMinusInfinity(alglibSolver.minusInfinity()));
+    iDynTree::assertTrue(problem->setPlusInfinity(alglibSolver.plusInfinity()));
+    iDynTree::assertTrue(alglibSolver.setProblem(problem));
+
+
+    for (int i = 0; i < 5; ++i){
+        iDynTree::assertTrue(alglibSolver.solve());
+        double optimalCost;
+        iDynTree::assertTrue(alglibSolver.getOptimalCost(optimalCost));
+        iDynTree::assertDoubleAreEqual(optimalCost, problem->expectedMinimum(), 1e-5);
+        iDynTree::VectorDynSize solution;
+        iDynTree::assertTrue(alglibSolver.getPrimalVariables(solution));
+        iDynTree::assertVectorAreEqual(solution, problem->expectedVariables(), 1e-5, "", 1);
+    }
+    return EXIT_SUCCESS;
+}
diff --git a/src/optimalcontrol/tests/CMakeLists.txt b/src/optimalcontrol/tests/CMakeLists.txt
index 32bb79f3ffa..fea8dee80fd 100644
--- a/src/optimalcontrol/tests/CMakeLists.txt
+++ b/src/optimalcontrol/tests/CMakeLists.txt
@@ -27,7 +27,7 @@ add_oc_test(OCProblem)
 add_oc_test(MultipleShooting)
 add_oc_test(L2Norm)
 if (IDYNTREE_USES_IPOPT)
-    add_oc_test(Optimizer)
+    add_oc_test(IpoptInterface)
     add_oc_test(OptimalControlIpopt)
 endif()
 
@@ -35,4 +35,11 @@ if (IDYNTREE_USES_OSQPEIGEN)
     add_oc_test(LinearOCOsqp)
 endif()
 
+if (IDYNTREE_USES_ALGLIB)
+    add_oc_test(AlglibInterface)
+endif()
+
+if (IDYNTREE_USES_WORHP)
+    add_oc_test(WorhpInterface)
+endif()
 
diff --git a/src/optimalcontrol/tests/OptimizerTest.cpp b/src/optimalcontrol/tests/IpoptInterfaceTest.cpp
similarity index 100%
rename from src/optimalcontrol/tests/OptimizerTest.cpp
rename to src/optimalcontrol/tests/IpoptInterfaceTest.cpp
diff --git a/src/optimalcontrol/tests/WorhpInterfaceTest.cpp b/src/optimalcontrol/tests/WorhpInterfaceTest.cpp
new file mode 100644
index 00000000000..918aa986dc8
--- /dev/null
+++ b/src/optimalcontrol/tests/WorhpInterfaceTest.cpp
@@ -0,0 +1,227 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/OptimizationProblem.h>
+#include <iDynTree/Optimizers/WorhpInterface.h>
+#include <iDynTree/Core/TestUtils.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <vector>
+#include <memory>
+
+class TestProblem : public iDynTree::optimization::OptimizationProblem {
+    double m_a1, m_b1, m_c1;
+    double m_a2, m_b2, m_c2;
+    double m_plusInfinity, m_minusInfinity;
+    iDynTree::VectorDynSize m_variables, m_expectedVariables;
+
+public:
+    TestProblem()
+    :m_plusInfinity(1e19)
+    ,m_minusInfinity(-1e19)
+    {}
+
+    virtual ~TestProblem() override {}
+
+    virtual bool prepare() override {
+        m_a1 = iDynTree::getRandomDouble(0.01, 10.0);
+        m_b1 = iDynTree::getRandomDouble(-10.0, 10.0);
+        m_c1 = iDynTree::getRandomDouble(-10.0, 10.0);
+        m_a2 = iDynTree::getRandomDouble(0.01, 10.0);
+        m_b2 = iDynTree::getRandomDouble(-10.0, 10.0);
+        m_c2 = iDynTree::getRandomDouble(-10.0, 10.0);
+
+        return true;
+    }
+
+    virtual void reset() override {}
+
+    virtual unsigned int numberOfVariables() override {
+        return 2;
+    }
+
+    virtual unsigned int numberOfConstraints() override {
+        return 2;
+    }
+
+    virtual bool getGuess(iDynTree::VectorDynSize &guess) override {
+        guess.resize(2);
+        guess.zero();
+        return true;
+    }
+
+    virtual bool getConstraintsBounds(iDynTree::VectorDynSize& constraintsLowerBounds, iDynTree::VectorDynSize& constraintsUpperBounds) override {
+        constraintsLowerBounds.resize(2);
+        constraintsUpperBounds.resize(2);
+        constraintsLowerBounds(0) = m_c1;
+        constraintsLowerBounds(1) = m_c2;
+        constraintsUpperBounds(0) = m_plusInfinity;
+        constraintsUpperBounds(1) = m_plusInfinity;
+        return true;
+    }
+
+    virtual bool getVariablesUpperBound(iDynTree::VectorDynSize& variablesUpperBound) override {
+        return false;
+    } //return false if not upper bounded
+
+    virtual bool getVariablesLowerBound(iDynTree::VectorDynSize& variablesLowerBound) override {
+        return false;
+    } //return false if not lower bounded
+
+    virtual bool getConstraintsJacobianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
+        nonZeroElementRows.resize(2);
+        nonZeroElementColumns.resize(2);
+
+        nonZeroElementRows[0] = 0;
+        nonZeroElementRows[1] = 1;
+
+        nonZeroElementColumns = nonZeroElementRows;
+
+        return true;
+    }
+
+    virtual bool getHessianInfo(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) override {
+        nonZeroElementRows.resize(2);
+        nonZeroElementColumns.resize(2);
+
+        nonZeroElementRows[0] = 0;
+        nonZeroElementRows[1] = 1;
+
+        nonZeroElementColumns = nonZeroElementRows;
+
+        return true;
+    } //costs and constraints together
+
+    virtual bool setVariables(const iDynTree::VectorDynSize& variables) override {
+        ASSERT_IS_TRUE(variables.size()==2);
+        m_variables = variables;
+        return true;
+    }
+
+    virtual bool evaluateCostFunction(double& costValue) override {
+        costValue = m_a1 * m_variables(0) * m_variables(0) + m_b1 * m_variables(0) + m_a2 * m_variables(1) * m_variables(1) + m_b2 * m_variables(1);
+        return true;
+    }
+
+    virtual bool evaluateCostGradient(iDynTree::VectorDynSize& gradient) override {
+        gradient.resize(numberOfVariables());
+        gradient(0) = 2 * m_a1 * m_variables(0) + m_b1;
+        gradient(1) = 2 * m_a2 * m_variables(1) + m_b2;
+        return true;
+    }
+
+    virtual bool evaluateCostHessian(iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(2,2);
+        hessian.zero();
+        hessian(0,0) = 2*m_a1;
+        hessian(1,1) = 2*m_a2;
+        return true;
+    }
+
+    virtual bool evaluateConstraints(iDynTree::VectorDynSize& constraints) override {
+        constraints.resize(2);
+        constraints = m_variables;
+        return true;
+    }
+
+    virtual bool evaluateConstraintsJacobian(iDynTree::MatrixDynSize& jacobian) override {
+        jacobian.resize(2,2);
+        jacobian(0,0) = 1.0;
+        jacobian(1,1) = 1.0;
+        return true;
+    } //using dense matrices, but the sparsity pattern is still obtained
+
+    virtual bool evaluateConstraintsHessian(const iDynTree::VectorDynSize& constraintsMultipliers, iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(2,2);
+        hessian.zero();
+        return true;
+    } //using dense matrices, but the sparsity pattern is still obtained
+
+    bool setPlusInfinity(double plusInfinity) {
+        if (plusInfinity < 0)
+            return false;
+
+        m_plusInfinity = plusInfinity;
+        return true;
+    }
+
+    bool setMinusInfinity(double minusInfinity) {
+        if (minusInfinity > 0)
+            return false;
+
+        m_minusInfinity = minusInfinity;
+        return true;
+    }
+
+    double expectedMinimum() {
+        double expected;
+        if (m_c1 < (-m_b1/(2*m_a1)))
+            expected = - m_b1 * m_b1 / (4*m_a1);
+        else
+            expected = m_a1*m_c1*m_c1 + m_b1*m_c1;
+
+        if (m_c2 < (-m_b2/(2*m_a2)))
+            expected += - m_b2 * m_b2 / (4*m_a2);
+        else
+            expected += m_a2*m_c2*m_c2 + m_b2*m_c2;
+
+        return expected;
+    }
+
+    const iDynTree::VectorDynSize& expectedVariables() {
+        m_expectedVariables.resize(2);
+        if (m_c1 < (-m_b1/(2*m_a1)))
+            m_expectedVariables(0) = - m_b1 / (2*m_a1);
+        else
+            m_expectedVariables(0) = m_c1;
+
+        if (m_c2 < (-m_b2/(2*m_a2)))
+            m_expectedVariables(1) = - m_b2 / (2*m_a2);
+        else
+            m_expectedVariables(1) = m_c2;
+
+        return m_expectedVariables;
+    }
+};
+
+int main(){
+    iDynTree::optimization::WorhpInterface worhpSolver;
+    std::shared_ptr<TestProblem> problem(new TestProblem);
+    iDynTree::VectorDynSize guess(2), dummy1, dummy2, dummy3;
+    guess.zero();
+
+    ASSERT_IS_TRUE(worhpSolver.setWorhpParam("AcceptTolOpti", 1e-6));
+    ASSERT_IS_TRUE(worhpSolver.setWorhpParam("AcceptTolFeas", 1e-6));
+    ASSERT_IS_TRUE(worhpSolver.setWorhpParam("Algorithm", 2));
+    ASSERT_IS_TRUE(worhpSolver.setWorhpParam("NLPprint", -1));
+
+
+    ASSERT_IS_TRUE(problem->setMinusInfinity(worhpSolver.minusInfinity()));
+    ASSERT_IS_TRUE(problem->setPlusInfinity(worhpSolver.plusInfinity()));
+    ASSERT_IS_TRUE(worhpSolver.setProblem(problem));
+    for (int i = 0; i < 5; ++i){
+        ASSERT_IS_TRUE(worhpSolver.solve());
+        double optimalCost;
+        ASSERT_IS_TRUE(worhpSolver.getOptimalCost(optimalCost));
+        ASSERT_EQUAL_DOUBLE_TOL(optimalCost, problem->expectedMinimum(), 1e-5);
+        iDynTree::VectorDynSize solution;
+        ASSERT_IS_TRUE(worhpSolver.getPrimalVariables(solution));
+        ASSERT_EQUAL_VECTOR_TOL(solution, problem->expectedVariables(), 1e-5);
+        ASSERT_IS_TRUE(worhpSolver.getDualVariables(dummy1, dummy2, dummy3));
+    }
+    return EXIT_SUCCESS;
+}

From d15b61928b8ac5364a246fcf9f7ebb38c3f1cda2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 15 Oct 2018 13:48:55 +0200
Subject: [PATCH 066/194] Fixed bug when computing cost gradients in optimal
 control.

---
 .../src/OptimalControlProblem.cpp             | 20 +++++++++----------
 1 file changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index e3bd13ae887..2ad083261d8 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -794,10 +794,10 @@ namespace iDynTree {
                         return false;
                     }
                     if (first){
-                        partialDerivative = m_pimpl->costStateJacobianBuffer;
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costStateJacobianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += toEigen(m_pimpl->costStateJacobianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costStateJacobianBuffer);
                     }
                 }
             }
@@ -831,10 +831,10 @@ namespace iDynTree {
                         return false;
                     }
                     if (first){
-                        partialDerivative = m_pimpl->costControlJacobianBuffer;
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costControlJacobianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += toEigen(m_pimpl->costControlJacobianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costControlJacobianBuffer);
                     }
                 }
             }
@@ -869,10 +869,10 @@ namespace iDynTree {
                         return false;
                     }
                     if (first){
-                        partialDerivative = m_pimpl->costStateHessianBuffer;
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costStateHessianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += toEigen(m_pimpl->costStateHessianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costStateHessianBuffer);
                     }
                 }
             }
@@ -908,10 +908,10 @@ namespace iDynTree {
                     }
 
                     if (first){
-                        partialDerivative = m_pimpl->costControlHessianBuffer;
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costControlHessianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += toEigen(m_pimpl->costControlHessianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costControlHessianBuffer);
                     }
                 }
             }
@@ -947,10 +947,10 @@ namespace iDynTree {
                     }
 
                     if (first){
-                        partialDerivative = m_pimpl->costMixedHessianBuffer;
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costMixedHessianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += toEigen(m_pimpl->costMixedHessianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costMixedHessianBuffer);
                     }
                 }
             }

From 53e20353a231858de9fe2fcda7f8832e2bbb9181 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 15 Oct 2018 14:12:48 +0200
Subject: [PATCH 067/194] Improvements on the WORHP and ALGLIB interface.

Better handling of error message for unsuccessfull solve in WORHP.

Specified the necessary ALGLIB version.

Using iDynTree namespace in notImplementedInterface.
---
 cmake/iDynTreeDependencies.cmake              |  2 +-
 .../iDynTree/Optimizers/AlglibInterface.h     |  7 ++-
 src/optimalcontrol/src/AlglibInterface.cpp    | 45 ++++++-------------
 .../src/AlglibInterfaceNotImplemented.cpp     | 41 +++++++++++++----
 src/optimalcontrol/src/WorhpInterface.cpp     |  4 ++
 .../src/WorhpInterfaceNotImplemented.cpp      |  2 +
 6 files changed, 57 insertions(+), 44 deletions(-)

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 6696d2ae6b8..6aa68f6d6c2 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -66,5 +66,5 @@ idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
-idyntree_handle_dependency(ALGLIB)
+idyntree_handle_dependency(ALGLIB 3.14.0)
 
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
index 23e65f7ce20..f9f0885902a 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
@@ -18,6 +18,7 @@
 #define IDYNTREE_OPTIMALCONTROL_ALGLIBINTERFACE_H
 
 #include <iDynTree/Optimizer.h>
+#include <memory>
 
 namespace iDynTree {
 
@@ -33,7 +34,7 @@ namespace iDynTree {
         class AlglibInterface : public Optimizer {
 
             class AlglibInterfaceImplementation;
-            AlglibInterfaceImplementation *m_pimpl;
+            std::unique_ptr<AlglibInterfaceImplementation> m_pimpl;
 
         public:
 
@@ -71,11 +72,9 @@ namespace iDynTree {
             //See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlcsetalgoaul
             bool setOuterIterations(unsigned int outerIterations);
 
-            //epsG: stopping criterion on the (scaled) cost gradient. Stops if its norm is lower than epsG.
-            //epsF: stopping criterion on the variation of the cost function
             //epsX: stopping criterion on the variation of optimization variables
             //See http://www.alglib.net/translator/man/manual.cpp.html#sub_minnlcsetcond
-            bool setStoppingConditions(double epsG, double epsF, double epsX);
+            bool setStoppingCondition(double epsX);
 
             bool setMaximumIterations(unsigned int maxIter);
 
diff --git a/src/optimalcontrol/src/AlglibInterface.cpp b/src/optimalcontrol/src/AlglibInterface.cpp
index f292d3cc8d4..de106c1c52c 100644
--- a/src/optimalcontrol/src/AlglibInterface.cpp
+++ b/src/optimalcontrol/src/AlglibInterface.cpp
@@ -21,7 +21,7 @@
 
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
-#include "iDynTree/Core/Utils.h"
+#include <iDynTree/Core/Utils.h>
 
 #include <optimization.h>
 #include <stdafx.h>
@@ -54,32 +54,35 @@ namespace iDynTree {
             alglib::minnlcreport alglibReport;
             alglib::real_1d_array alglibScaling;
             int exitCode;
-            double epsg;
-            double epsf;
             double epsx;
             alglib::ae_int_t maxits;
             alglib::ae_int_t outerits;
             double rho;
 
 
-            AlglibInterfaceImplementation(){
+            AlglibInterfaceImplementation() {
                 nlpData = new(SharedData);
                 assert(nlpData);
                 nlpData->problem = nullptr;
                 exitCode = -10;
-                epsg = 0;
-                epsf = 0;
                 epsx = 0.000001;
                 maxits = 0;
                 outerits = 5;
                 rho = 1000;
             }
 
+            ~AlglibInterfaceImplementation() {
+                if (nlpData){
+                    delete nlpData;
+                    nlpData = nullptr;
+                }
+            }
+
         };
 
         void ALGLIB_NLP(const alglib::real_1d_array &x, alglib::real_1d_array &fi, alglib::real_2d_array &jac, void *ptr){
             // See http://www.alglib.net/translator/man/manual.cpp.html#example_minnlc_d_inequality
-            SharedData* pimpl = (SharedData*) ptr;
+            SharedData* pimpl = reinterpret_cast<SharedData*>(ptr);
             assert(pimpl->problem);
             Eigen::Map<const Eigen::VectorXd> x_map(x.getcontent(), x.length());
             Eigen::Map<Eigen::VectorXd> f_map(fi.getcontent(), fi.length());
@@ -154,14 +157,6 @@ namespace iDynTree {
 
         AlglibInterface::~AlglibInterface()
         {
-            if (m_pimpl){
-                if (m_pimpl->nlpData){
-                    delete m_pimpl->nlpData;
-                    m_pimpl->nlpData = nullptr;
-                }
-                delete m_pimpl;
-                m_pimpl = nullptr;
-            }
 
         }
 
@@ -241,7 +236,7 @@ namespace iDynTree {
             unsigned int inequalities = 0, equalities = 0;
             ConstraintInfo newConstraint;
             for (unsigned int i=0; i < nc; ++i){
-                if (m_pimpl->constraintsLowerBounds(i) == m_pimpl->constraintsUpperBounds(i)){
+                if (checkDoublesAreEqual(m_pimpl->constraintsLowerBounds(i), m_pimpl->constraintsUpperBounds(i))){
                     newConstraint.boundValue = -1.0 * m_pimpl->constraintsUpperBounds(i);
                     newConstraint.constraintSign = +1.0;
                     newConstraint.index = i;
@@ -310,7 +305,7 @@ namespace iDynTree {
             }
 
             alglib::minnlcsetalgoaul(m_pimpl->alglibState, m_pimpl->rho, m_pimpl->outerits);
-            alglib::minnlcsetcond(m_pimpl->alglibState, m_pimpl->epsg, m_pimpl->epsf, m_pimpl->epsx, m_pimpl->maxits);
+            alglib::minnlcsetcond(m_pimpl->alglibState, m_pimpl->epsx, m_pimpl->maxits);
             alglib::minnlcsetscale(m_pimpl->alglibState, m_pimpl->alglibScaling);
             alglib::minnlcsetprecinexact(m_pimpl->alglibState);
 
@@ -438,25 +433,13 @@ namespace iDynTree {
             return true;
         }
 
-        bool AlglibInterface::setStoppingConditions(double epsG, double epsF, double epsX)
+        bool AlglibInterface::setStoppingCondition(double epsX)
         {
-            if (epsG < 0){
-                reportError("ALGLIBInterface", "setStoppingConditions", "The epsG parameter is supposed to be non negative");
-                return false;
-            }
-
-            if (epsF < 0){
-                reportError("ALGLIBInterface", "setStoppingConditions", "The epsF parameter is supposed to be non negative");
-                return false;
-            }
-
             if (epsX < 0){
-                reportError("ALGLIBInterface", "setStoppingConditions", "The epsX parameter is supposed to be non negative");
+                reportError("ALGLIBInterface", "setStoppingCondition", "The epsX parameter is supposed to be non negative");
                 return false;
             }
 
-            m_pimpl->epsg = epsG;
-            m_pimpl->epsf = epsF;
             m_pimpl->epsx = epsX;
 
             return true;
diff --git a/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp b/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp
index f74be18ef5d..4901545af67 100644
--- a/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/AlglibInterfaceNotImplemented.cpp
@@ -18,6 +18,7 @@
 #include <iDynTree/Core/Utils.h>
 
 using namespace iDynTree::optimization;
+using namespace iDynTree;
 
 AlglibInterface::AlglibInterface()
 {
@@ -36,50 +37,74 @@ bool AlglibInterface::isAvailable() const
 
 bool AlglibInterface::setProblem(std::shared_ptr<OptimizationProblem> problem)
 {
-    reportError("AlglibInterface", "setProblem", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "setProblem", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return false;
 }
 
 bool AlglibInterface::solve()
 {
-    reportError("AlglibInterface", "solve", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "solve", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return false;
 }
 
 bool AlglibInterface::getPrimalVariables(VectorDynSize &primalVariables)
 {
-    reportError("AlglibInterface", "getPrimalVariables", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "getPrimalVariables", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return false;
 }
 
 bool AlglibInterface::getDualVariables(VectorDynSize &constraintsMultipliers, VectorDynSize &lowerBoundsMultipliers, VectorDynSize &upperBoundsMultipliers)
 {
-    reportError("AlglibInterface", "getDualVariables", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "getDualVariables", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return false;
 }
 
 bool AlglibInterface::getOptimalCost(double &optimalCost)
 {
-    reportError("AlglibInterface", "getOptimalCost", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "getOptimalCost", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return false;
 }
 
 bool AlglibInterface::getOptimalConstraintsValues(VectorDynSize &constraintsValues)
 {
-    reportError("AlglibInterface", "getOptimalConstraintsValues", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "getOptimalConstraintsValues", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return false;
 }
 
 double AlglibInterface::minusInfinity()
 {
-    reportError("AlglibInterface", "minusInfinity", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "minusInfinity", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return Optimizer::minusInfinity();
 }
 
 double AlglibInterface::plusInfinity()
 {
-    reportError("AlglibInterface", "plusInfinity", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+    reportError("AlglibInterface", "plusInfinity", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
     return Optimizer::plusInfinity();
 }
 
+bool AlglibInterface::setRHO(double rho)
+{
+    reportError("AlglibInterface", "setRHO", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
+    return false;
+}
+
+bool AlglibInterface::setOuterIterations(unsigned int outerIterations)
+{
+    reportError("AlglibInterface", "setOuterIterations", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
+    return false;
+}
+
+bool AlglibInterface::setStoppingCondition(double epsX)
+{
+    reportError("AlglibInterface", "setStoppingCondition", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
+    return false;
+}
+
+bool AlglibInterface::setMaximumIterations(unsigned int maxIter)
+{
+    reportError("AlglibInterface", "setMaximumIterations", "AlglibInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_ALGLIB set to ON?");
+    return false;
+}
+
 
diff --git a/src/optimalcontrol/src/WorhpInterface.cpp b/src/optimalcontrol/src/WorhpInterface.cpp
index 3697254ceb9..f3bc657bf9c 100644
--- a/src/optimalcontrol/src/WorhpInterface.cpp
+++ b/src/optimalcontrol/src/WorhpInterface.cpp
@@ -655,7 +655,11 @@ bool WorhpInterface::solve()
     if (m_pimpl->worhp.cnt.status <= TerminateError) {
         reportError("WorhpInterface", "solve", "Failed to solve the problem. Here additional infos:");
         m_pimpl->previouslySolved = false;
+        int printLevel;
+        getWorhpParam("NLPprint", printLevel);
+        setWorhpParam("NLPprint", 0);
         StatusMsg(&m_pimpl->worhp.opt, &m_pimpl->worhp.wsp, &m_pimpl->worhp.par, &m_pimpl->worhp.cnt);
+        setWorhpParam("NLPprint", printLevel);
         return false;
     }
     m_pimpl->previouslySolved = true;
diff --git a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
index 782f6b71344..2f5db60a2a5 100644
--- a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
@@ -18,6 +18,8 @@
 #include <iDynTree/Core/Utils.h>
 
 using namespace iDynTree::optimization;
+using namespace iDynTree;
+
 
 WorhpInterface::WorhpInterface()
 {

From 95c864457df13cb50c00b44d3f713b6ba023a394 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 5 Nov 2018 12:23:27 +0100
Subject: [PATCH 068/194] Using target properties instead of propagating
 variables for ALGLIB and WORHP.

---
 cmake/FindWORHP.cmake             | 21 +++++++++++++++++++++
 cmake/iDynTreeDependencies.cmake  | 13 +------------
 src/optimalcontrol/CMakeLists.txt |  6 ++----
 3 files changed, 24 insertions(+), 16 deletions(-)
 create mode 100644 cmake/FindWORHP.cmake

diff --git a/cmake/FindWORHP.cmake b/cmake/FindWORHP.cmake
new file mode 100644
index 00000000000..683074c5822
--- /dev/null
+++ b/cmake/FindWORHP.cmake
@@ -0,0 +1,21 @@
+include(FindPackageHandleStandardArgs)
+
+find_path(WORHP_INCLUDE_DIR worhp HINTS ENV WORHP_DIR)
+mark_as_advanced(WORHP_INCLUDE_DIR)
+find_library(WORHP_LIBRARY worhp HINTS ENV WORHP_DIR)
+mark_as_advanced(WORHP_LIBRARY)
+
+find_package_handle_standard_args(WORHP DEFAULT_MSG WORHP_INCLUDE_DIR WORHP_LIBRARY)
+
+if(WORHP_FOUND)
+    set(WORHP_INCLUDE_DIRS ${WORHP_INCLUDE_DIR})
+    set(WORHP_LIBRARIES ${WORHP_LIBRARY})
+
+    if(NOT TARGET WORHP::WORHP)
+      add_library(WORHP::WORHP UNKNOWN IMPORTED)
+      set_target_properties(WORHP::WORHP PROPERTIES
+        INTERFACE_INCLUDE_DIRECTORIES "${WORHP_INCLUDE_DIR}")
+      set_property(TARGET WORHP::WORHP APPEND PROPERTY
+        IMPORTED_LOCATION "${WORHP_LIBRARY}")
+    endif()
+endif()
diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 6aa68f6d6c2..0e35ef70fd5 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -51,20 +51,9 @@ if(IDYNTREE_USES_YARP AND YARP_FOUND)
   endif()
 endif()
 
-find_library(WORHP_LIB worhp HINTS ENV WORHP_DIR)
-option(IDYNTREE_USES_WORHP "Use WORHP as a solver in optimalcontrol." WORHP_LIB)
-if (IDYNTREE_USES_WORHP)
-    if (NOT WORHP_LIB)
-        message(FATAL_ERROR "The IDYNTREE_USES_WORHP option was selected, but it was not possible to find WORHP.")
-    endif()
-
-    find_path(WORHP_INCLUDE_DIRS worhp HINTS ENV WORHP_DIR)
-
-endif()
-
 idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
 idyntree_handle_dependency(ALGLIB 3.14.0)
-
+idyntree_handle_dependency(WORHP)
diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index d84bbb52070..041504e49df 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -93,8 +93,7 @@ endif()
 list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/AlglibInterface.h)
 if (IDYNTREE_USES_ALGLIB)
     list(APPEND OPTIMIZERS_SOURCES src/AlglibInterface.cpp)
-    list(APPEND LINK_LIST ${ALGLIB_LIB})
-    list(APPEND INCLUDE_LIST ${ALGLIB_INCLUDE_DIRS})
+    list(APPEND LINK_LIST ALGLIB)
 else()
     list(APPEND OPTIMIZERS_SOURCES src/AlglibInterfaceNotImplemented.cpp)
 endif()
@@ -102,8 +101,7 @@ endif()
 list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/WorhpInterface.h)
 if (IDYNTREE_USES_WORHP)
     list(APPEND OPTIMIZERS_SOURCES src/WorhpInterface.cpp)
-    list(APPEND LINK_LIST ${WORHP_LIB})
-    list(APPEND INCLUDE_LIST ${WORHP_INCLUDE_DIRS})
+    list(APPEND LINK_LIST WORHP::WORHP)
 else()
     list(APPEND OPTIMIZERS_SOURCES src/WorhpInterfaceNotImplemented.cpp)
 endif()

From 7d1bed22bdba49d6591b3d5ceeae42ae4db01402 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 5 Nov 2018 12:34:14 +0100
Subject: [PATCH 069/194] Updated docs

Revert "Use unique_ptr for ALGLIB implementation."

The problem is related to the fact that we cannot use a unique_ptr with the notImplemented interface.
---
 README.md                                       | 11 ++++++++---
 doc/releases/v0_12.md                           |  3 ++-
 .../iDynTree/Optimizers/AlglibInterface.h       |  3 +--
 src/optimalcontrol/src/AlglibInterface.cpp      | 17 +++++++++--------
 4 files changed, 20 insertions(+), 14 deletions(-)

diff --git a/README.md b/README.md
index ac816d2e8b2..909b4ed0e9b 100644
--- a/README.md
+++ b/README.md
@@ -40,6 +40,11 @@ but you can manually make sure that iDynTree searches or ignores a given depende
 - [YARP](https://github.com/robotology/yarp)
 - [ICUB](https://github.com/robotology/icub-main)
 
+##### Optional for the optimal control part
+- [ALGLIB](https://github.com/S-Dafarra/alglib-cmake)
+- [OSQP](https://github.com/robotology/osqp-eigen)
+- [WORHP](https://worhp.de/)
+
 ##### Deprecated
 - [Kinematics and Dynamics Library](https://github.com/orocos/orocos_kinematics_dynamics)
 - [urdfdom](https://github.com/ros/urdfdom)
@@ -177,7 +182,7 @@ The documentation for the complete API of iDynTree is automatically extracted fr
 and is available at the URL : [http://robotology.gitlab.io/docs/idyntree/master/](http://robotology.gitlab.io/docs/idyntree/master/).
 The documentation generated from the `devel` branch is available at the URL : [http://robotology.gitlab.io/docs/idyntree/devel/](http://robotology.gitlab.io/docs/idyntree/devel/).
 
-## Announcements 
+## Announcements
 Announcements on new releases, API changes or other news are done on [`robotology/QA` GitHub repository](https://github.com/robotology/QA). You can watch that repository to get all the iDynTree-related announcements, that will always tagged with the `announcement` tag.
 
 ## Developer Documentation
@@ -209,8 +214,8 @@ The initial development of iDynTree was supported by the FP7 EU projects [CoDyCo
 
 The development is now supported by the [Dynamic Interaction Control research line](https://www.iit.it/research/lines/dynamic-interaction-control) at the [Italian Institute of Technology](https://www.iit.it/).
 
-## License 
-iDynTree is licensed under either the GNU Lesser General Public License v3.0 : 
+## License
+iDynTree is licensed under either the GNU Lesser General Public License v3.0 :
 
 https://www.gnu.org/licenses/lgpl-3.0.html
 
diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 763038e0937..5dcf7711164 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -8,7 +8,7 @@ iDynTree 0.12 Release Notes
 
 Summary
 -------
-* 
+*
 
 Important Changes
 -----------------
@@ -25,6 +25,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added ``OSQP`` interface via ``osqp-eigen``.
 * Fixed bugs in ``MultipleShooting`` solver.
 * Added few lines of documentation.
+* Added interface for ``ALGLIB`` and ``WORHP``.
 
 #### `visualization`
 * Added visualization of vectors.
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
index f9f0885902a..a2a7ed8b227 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/AlglibInterface.h
@@ -18,7 +18,6 @@
 #define IDYNTREE_OPTIMALCONTROL_ALGLIBINTERFACE_H
 
 #include <iDynTree/Optimizer.h>
-#include <memory>
 
 namespace iDynTree {
 
@@ -34,7 +33,7 @@ namespace iDynTree {
         class AlglibInterface : public Optimizer {
 
             class AlglibInterfaceImplementation;
-            std::unique_ptr<AlglibInterfaceImplementation> m_pimpl;
+            AlglibInterfaceImplementation *m_pimpl;
 
         public:
 
diff --git a/src/optimalcontrol/src/AlglibInterface.cpp b/src/optimalcontrol/src/AlglibInterface.cpp
index de106c1c52c..4e1b8c07931 100644
--- a/src/optimalcontrol/src/AlglibInterface.cpp
+++ b/src/optimalcontrol/src/AlglibInterface.cpp
@@ -60,7 +60,7 @@ namespace iDynTree {
             double rho;
 
 
-            AlglibInterfaceImplementation() {
+            AlglibInterfaceImplementation(){
                 nlpData = new(SharedData);
                 assert(nlpData);
                 nlpData->problem = nullptr;
@@ -71,13 +71,6 @@ namespace iDynTree {
                 rho = 1000;
             }
 
-            ~AlglibInterfaceImplementation() {
-                if (nlpData){
-                    delete nlpData;
-                    nlpData = nullptr;
-                }
-            }
-
         };
 
         void ALGLIB_NLP(const alglib::real_1d_array &x, alglib::real_1d_array &fi, alglib::real_2d_array &jac, void *ptr){
@@ -157,6 +150,14 @@ namespace iDynTree {
 
         AlglibInterface::~AlglibInterface()
         {
+            if (m_pimpl){
+                if (m_pimpl->nlpData){
+                    delete m_pimpl->nlpData;
+                    m_pimpl->nlpData = nullptr;
+                }
+                delete m_pimpl;
+                m_pimpl = nullptr;
+            }
 
         }
 

From 21ba16cf1729347e7b4f1d4a48322c1320136ab6 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 11 Jan 2019 14:28:10 +0100
Subject: [PATCH 070/194] Ignoring all txt.user files.

---
 .gitignore | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index 65e0d549195..61e18ddb31d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,5 +5,5 @@ build/*
 **/build/*
 
 .DS_Store
-*.txt.user
+*.txt.user*
 .idea/*

From 809a2dd892e06bd25169b4ea6ed5d232400d2183 Mon Sep 17 00:00:00 2001
From: Prashanth Ramadoss <prashanthramadoss@gmail.com>
Date: Mon, 14 Jan 2019 13:20:47 +0100
Subject: [PATCH 071/194] [visualization] set/reset link color, visualizer
 world transform

- methods to set/reset link geometry colors
- methods to get the pose of the model or links with respect t visualizer world
---
 .../include/iDynTree/Visualizer.h             |  33 +++-
 src/visualization/src/DummyImplementations.h  |   7 +-
 src/visualization/src/IrrlichtUtils.h         |  12 ++
 src/visualization/src/ModelVisualization.cpp  | 174 +++++++++++-------
 src/visualization/src/ModelVisualization.h    |   4 +
 5 files changed, 160 insertions(+), 70 deletions(-)

diff --git a/src/visualization/include/iDynTree/Visualizer.h b/src/visualization/include/iDynTree/Visualizer.h
index 0a1ce1b917c..b31c76938bb 100644
--- a/src/visualization/include/iDynTree/Visualizer.h
+++ b/src/visualization/include/iDynTree/Visualizer.h
@@ -406,8 +406,21 @@ class IModelVisualization
     /**
      * Reset the colors of the model.
      */
-    virtual void resetModelColor() = 0;
-
+    virtual void resetModelColor() = 0;    
+    
+    /**
+     * Set the color of all the geometries of the given link.
+     *
+     * This will overwrite the material of the link, but it can be
+     * reset by resetLinkColor.
+     */
+    virtual bool setLinkColor(const LinkIndex& linkIndex, const ColorViz& linkColor) = 0;
+    
+    /**
+     * Reset the colors of given link.
+     */
+    virtual bool resetLinkColor(const LinkIndex& linkIndex) = 0;
+    
     /**
      * Get the name of the link in the model.
      */
@@ -437,8 +450,20 @@ class IModelVisualization
      * Get a reference to the internal IJetsVisualization interface.
      */
     virtual IJetsVisualization& jets() = 0;
-
-
+        
+    /**
+     * Get the transformation of the model (root link) with respect to visualizer world \f$ w_H_{root}\f$
+     * The obtained transformation matrix can be used to map any homogeneous vector from the
+     * model's root link frame to the visualizer world frame.
+     */
+    virtual Transform getWorldModelTransform() = 0;
+    
+    /**
+     * Get the transformation of given link with respect to visualizer world \f$ w_H_{link}\f$
+     * The obtained transformation matrix can be used to map any homogeneous vector from the
+     * given link frame to the visualizer world frame.
+     */
+    virtual Transform getWorldLinkTransform(const LinkIndex& linkIndex) = 0;
 };
 
 /**
diff --git a/src/visualization/src/DummyImplementations.h b/src/visualization/src/DummyImplementations.h
index 64abc199e31..d81bd76b62c 100644
--- a/src/visualization/src/DummyImplementations.h
+++ b/src/visualization/src/DummyImplementations.h
@@ -120,13 +120,16 @@ class DummyModelVisualization : public IModelVisualization
     virtual std::string getInstanceName() { return "dummyModelVisualizationInstance"; }
     virtual void setModelVisibility(const bool) {}
     virtual void setModelColor(const ColorViz & ) {}
-    virtual void resetModelColor() {}
+    virtual void resetModelColor() {}   
+    virtual bool setLinkColor(const LinkIndex &, const ColorViz &) { return false; }
+    virtual bool resetLinkColor(const LinkIndex &) { return false; }
     virtual std::vector< std::string > getLinkNames() { return std::vector<std::string>(); };
     virtual bool setLinkVisibility(const std::string &, bool) { return false; }
     virtual std::vector<std::string> getFeatures() { return std::vector<std::string>(); }
     virtual bool setFeatureVisibility(const std::string& , bool) { return false; }
     virtual IJetsVisualization& jets() { return m_dummyJets;  }
-
+    virtual Transform getWorldModelTransform() { return iDynTree::Transform::Identity(); }
+    virtual Transform getWorldLinkTransform(const LinkIndex &) { return iDynTree::Transform::Identity(); }
 };
 
 }
diff --git a/src/visualization/src/IrrlichtUtils.h b/src/visualization/src/IrrlichtUtils.h
index c06bdfc79c6..6d89a3f4d75 100644
--- a/src/visualization/src/IrrlichtUtils.h
+++ b/src/visualization/src/IrrlichtUtils.h
@@ -103,6 +103,18 @@ inline irr::video::SMaterial idyntree2irr(const iDynTree::ColorViz & rgbaMateria
     return idyntree2irr(vec4);
 }
 
+inline iDynTree::Transform irr2idyntree_trans(const irr::core::matrix4 & transIrr)
+{
+    iDynTree::Matrix4x4 trans;
+    for (int i = 0; i < 4; i++)
+    {
+        for (int j = 0; j < 4; j++)
+        {
+            trans(i, j) = transIrr(i, j);
+        }
+    }
+    return iDynTree::Transform(trans);
+}
 
 /**
  * Get a rotation matrix whose z column is the provided direction.
diff --git a/src/visualization/src/ModelVisualization.cpp b/src/visualization/src/ModelVisualization.cpp
index e35b5016e8e..666a44a6d36 100644
--- a/src/visualization/src/ModelVisualization.cpp
+++ b/src/visualization/src/ModelVisualization.cpp
@@ -80,7 +80,6 @@ struct ModelVisualization::ModelVisualizationPimpl
     }
 };
 
-
 /**
  * Add  iDynTree::ModelGeometries to an irr::scene::ISceneManager*
  * We create a SceneGraph for the URDF model of this type: the root object
@@ -213,6 +212,29 @@ Model& ModelVisualization::model()
     return this->pimpl->m_model;
 }
 
+Transform ModelVisualization::getWorldModelTransform()
+{
+    Transform w_H_b;
+    irr::core::matrix4 relativeTransform(this->pimpl->modelNode->getRelativeTransformation());    
+    w_H_b = irr2idyntree_trans(relativeTransform);
+    return w_H_b;
+}
+
+Transform ModelVisualization::getWorldLinkTransform(const LinkIndex& linkIndex)
+{
+    if (linkIndex < 0 || linkIndex >= pimpl->geomNodes.size())
+    {
+        reportError("ModelVisualization","getWorldToLinkTransorm", "invalid link index. returning identity transform");
+        return Transform::Identity();
+    }
+    
+    Transform w_H_link;
+    irr::core::matrix4 relativeLinkTransform(this->pimpl->linkNodes[linkIndex]->getRelativeTransformation());
+    w_H_link = irr2idyntree_trans(relativeLinkTransform);
+    return w_H_link;
+}
+
+
 bool ModelVisualization::setPositions(const Transform& world_H_base, const VectorDynSize& jointPos)
 {
     if( (jointPos.size() != model().getNrOfPosCoords()) )
@@ -265,85 +287,109 @@ void ModelVisualization::setModelVisibility(const bool isVisible)
     }
 }
 
-void ModelVisualization::setModelColor(const ColorViz& modelColor)
+bool ModelVisualization::setLinkColor(const LinkIndex& linkIndex, const ColorViz& linkColor)
 {
-    irr::video::SColor col = idyntree2irrlicht(modelColor).toSColor();
-
-    for(size_t linkIdx=0; linkIdx < pimpl->geomNodes.size(); linkIdx++)
+    if (linkIndex < 0 || linkIndex >= pimpl->geomNodes.size())
     {
-        for(size_t geom=0; geom < pimpl->geomNodes[linkIdx].size(); geom++)
+        reportError("ModelVisualization","setLinkColor", "invalid link index");
+        return false;
+    }
+        
+    irr::video::SColor col = idyntree2irrlicht(linkColor).toSColor();
+    for(size_t geom=0; geom < pimpl->geomNodes[linkIndex].size(); geom++)
+    {
+        if( pimpl->geomNodes[linkIndex][geom] )
         {
-            if( pimpl->geomNodes[linkIdx][geom] )
-            {
-                irr::scene::ISceneNode * geomNode = pimpl->geomNodes[linkIdx][geom];
+            irr::scene::ISceneNode * geomNode = pimpl->geomNodes[linkIndex][geom];
 
-                for( size_t mat = 0; mat < geomNode->getMaterialCount(); mat++)
-                {
-                    irr::video::SMaterial geomMat = geomNode->getMaterial(mat);
-
-                    // R
-                    geomMat.AmbientColor.setRed(col.getRed());
-                    geomMat.DiffuseColor.setRed(col.getRed());
-                    geomMat.SpecularColor.setRed(col.getRed());
-                    geomMat.EmissiveColor.setRed(col.getRed());
-
-                    // G
-                    geomMat.AmbientColor.setGreen(col.getGreen());
-                    geomMat.DiffuseColor.setGreen(col.getGreen());
-                    geomMat.SpecularColor.setGreen(col.getGreen());
-                    geomMat.EmissiveColor.setGreen(col.getGreen());
-
-                    // B
-                    geomMat.AmbientColor.setBlue(col.getBlue());
-                    geomMat.DiffuseColor.setBlue(col.getBlue());
-                    geomMat.SpecularColor.setBlue(col.getBlue());
-                    geomMat.EmissiveColor.setBlue(col.getBlue());
-
-                    geomNode->getMaterial(mat) = geomMat;
-                }
+            for( size_t mat = 0; mat < geomNode->getMaterialCount(); mat++)
+            {
+                irr::video::SMaterial geomMat = geomNode->getMaterial(mat);
+
+                // R
+                geomMat.AmbientColor.setRed(col.getRed());
+                geomMat.DiffuseColor.setRed(col.getRed());
+                geomMat.SpecularColor.setRed(col.getRed());
+                geomMat.EmissiveColor.setRed(col.getRed());
+
+                // G
+                geomMat.AmbientColor.setGreen(col.getGreen());
+                geomMat.DiffuseColor.setGreen(col.getGreen());
+                geomMat.SpecularColor.setGreen(col.getGreen());
+                geomMat.EmissiveColor.setGreen(col.getGreen());
+
+                // B
+                geomMat.AmbientColor.setBlue(col.getBlue());
+                geomMat.DiffuseColor.setBlue(col.getBlue());
+                geomMat.SpecularColor.setBlue(col.getBlue());
+                geomMat.EmissiveColor.setBlue(col.getBlue());
+
+                geomNode->getMaterial(mat) = geomMat;
             }
         }
     }
+    return true;
 }
 
-void ModelVisualization::resetModelColor()
+bool ModelVisualization::resetLinkColor(const LinkIndex& linkIndex)
 {
-    for(size_t linkIdx=0; linkIdx < pimpl->geomNodes.size(); linkIdx++)
+    if (linkIndex < 0 || linkIndex >= pimpl->geomNodes.size())
     {
-        for(size_t geom=0; geom < pimpl->geomNodes[linkIdx].size(); geom++)
+        reportError("ModelVisualization","resetLinkColor", "invalid link index");
+        return false;
+    }
+    
+    for(size_t geom=0; geom < pimpl->geomNodes[linkIndex].size(); geom++)
+    {
+        if( pimpl->geomNodes[linkIndex][geom] )
         {
-            if( pimpl->geomNodes[linkIdx][geom] )
-            {
-                irr::scene::ISceneNode * geomNode = pimpl->geomNodes[linkIdx][geom];
-                std::vector< irr::video::SMaterial > & materialCache = pimpl->geomNodesNotTransparentMaterialCache[linkIdx][geom];
+            irr::scene::ISceneNode * geomNode = pimpl->geomNodes[linkIndex][geom];
+            std::vector< irr::video::SMaterial > & materialCache = pimpl->geomNodesNotTransparentMaterialCache[linkIndex][geom];
 
-                for( size_t mat = 0; mat < geomNode->getMaterialCount(); mat++)
-                {
-                    irr::video::SMaterial geomMat = geomNode->getMaterial(mat);
-
-                    // R
-                    geomMat.AmbientColor.setRed(materialCache[mat].AmbientColor.getRed());
-                    geomMat.DiffuseColor.setRed(materialCache[mat].DiffuseColor.getRed());
-                    geomMat.SpecularColor.setRed(materialCache[mat].SpecularColor.getRed());
-                    geomMat.EmissiveColor.setRed(materialCache[mat].EmissiveColor.getRed());
-
-                    // G
-                    geomMat.AmbientColor.setGreen(materialCache[mat].AmbientColor.getGreen());
-                    geomMat.DiffuseColor.setGreen(materialCache[mat].DiffuseColor.getGreen());
-                    geomMat.SpecularColor.setGreen(materialCache[mat].SpecularColor.getGreen());
-                    geomMat.EmissiveColor.setGreen(materialCache[mat].EmissiveColor.getGreen());
-
-                    // B
-                    geomMat.AmbientColor.setBlue(materialCache[mat].AmbientColor.getBlue());
-                    geomMat.DiffuseColor.setBlue(materialCache[mat].DiffuseColor.getBlue());
-                    geomMat.SpecularColor.setBlue(materialCache[mat].SpecularColor.getBlue());
-                    geomMat.EmissiveColor.setBlue(materialCache[mat].EmissiveColor.getBlue());
-
-                    geomNode->getMaterial(mat) = geomMat;
-                }
+            for( size_t mat = 0; mat < geomNode->getMaterialCount(); mat++)
+            {
+                irr::video::SMaterial geomMat = geomNode->getMaterial(mat);
+
+                // R
+                geomMat.AmbientColor.setRed(materialCache[mat].AmbientColor.getRed());
+                geomMat.DiffuseColor.setRed(materialCache[mat].DiffuseColor.getRed());
+                geomMat.SpecularColor.setRed(materialCache[mat].SpecularColor.getRed());
+                geomMat.EmissiveColor.setRed(materialCache[mat].EmissiveColor.getRed());
+
+                // G
+                geomMat.AmbientColor.setGreen(materialCache[mat].AmbientColor.getGreen());
+                geomMat.DiffuseColor.setGreen(materialCache[mat].DiffuseColor.getGreen());
+                geomMat.SpecularColor.setGreen(materialCache[mat].SpecularColor.getGreen());
+                geomMat.EmissiveColor.setGreen(materialCache[mat].EmissiveColor.getGreen());
+
+                // B
+                geomMat.AmbientColor.setBlue(materialCache[mat].AmbientColor.getBlue());
+                geomMat.DiffuseColor.setBlue(materialCache[mat].DiffuseColor.getBlue());
+                geomMat.SpecularColor.setBlue(materialCache[mat].SpecularColor.getBlue());
+                geomMat.EmissiveColor.setBlue(materialCache[mat].EmissiveColor.getBlue());
+
+                geomNode->getMaterial(mat) = geomMat;
             }
         }
     }
+   return true; 
+}
+
+
+void ModelVisualization::setModelColor(const ColorViz& modelColor)
+{
+    for(size_t linkIdx=0; linkIdx < pimpl->geomNodes.size(); linkIdx++)
+    {
+        this->setLinkColor(linkIdx, modelColor);
+    }
+}
+
+void ModelVisualization::resetModelColor()
+{
+    for(size_t linkIdx=0; linkIdx < pimpl->geomNodes.size(); linkIdx++)
+    {
+        this->resetLinkColor(linkIdx);
+    }
 }
 
 std::vector< std::string > ModelVisualization::getLinkNames()
diff --git a/src/visualization/src/ModelVisualization.h b/src/visualization/src/ModelVisualization.h
index d901f5a7700..0a797ce2f6c 100644
--- a/src/visualization/src/ModelVisualization.h
+++ b/src/visualization/src/ModelVisualization.h
@@ -40,6 +40,8 @@ class ModelVisualization: public IModelVisualization
     virtual void setModelVisibility(const bool isVisible);
     virtual void setModelColor(const ColorViz & modelColor);
     virtual void resetModelColor();
+    virtual bool setLinkColor(const LinkIndex& linkIndex, const ColorViz& linkColor);
+    virtual bool resetLinkColor(const LinkIndex& linkIndex); 
     virtual std::vector< std::string > getLinkNames();
     virtual bool setLinkVisibility(const std::string & linkName, bool isVisible);
     virtual std::vector<std::string> getFeatures();
@@ -47,6 +49,8 @@ class ModelVisualization: public IModelVisualization
     void setWireframeVisibility(bool isVisible);
     void setTransparent(bool isTransparent);
     virtual IJetsVisualization& jets();
+    virtual Transform getWorldModelTransform();
+    virtual Transform getWorldLinkTransform(const LinkIndex& linkIndex);
 };
 
 }

From e543d3c2b23d5f95aa69f07deb2d522f3d2ff9f5 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 15 Oct 2018 18:09:13 +0200
Subject: [PATCH 072/194] Implemented finer sparsity in constraints and
 integrators.

---
 src/core/include/iDynTree/Core/Utils.h        |  3 +
 src/core/src/Utils.cpp                        | 11 +++
 .../include/iDynTree/Constraint.h             | 20 ++++++
 .../include/iDynTree/ConstraintsGroup.h       | 18 +++++
 .../include/iDynTree/DynamicalSystem.h        | 21 +++++-
 .../include/iDynTree/Integrator.h             | 13 +++-
 .../iDynTree/Integrators/ForwardEuler.h       |  8 +++
 .../Integrators/ImplicitTrapezoidal.h         | 26 ++++---
 src/optimalcontrol/src/Constraint.cpp         | 10 +++
 src/optimalcontrol/src/ConstraintsGroup.cpp   | 70 ++++++++++++++++++-
 src/optimalcontrol/src/DynamicalSystem.cpp    | 10 +++
 src/optimalcontrol/src/ForwardEuler.cpp       | 43 ++++++++++++
 .../src/ImplicitTrapezoidal.cpp               | 50 +++++++++++--
 src/optimalcontrol/src/Integrator.cpp         | 10 +++
 14 files changed, 294 insertions(+), 19 deletions(-)

diff --git a/src/core/include/iDynTree/Core/Utils.h b/src/core/include/iDynTree/Core/Utils.h
index a5405d0d030..b45ab208dfe 100644
--- a/src/core/include/iDynTree/Core/Utils.h
+++ b/src/core/include/iDynTree/Core/Utils.h
@@ -12,6 +12,7 @@
 #define IDYNTREE_UTILS_H
 
 #include <cstddef>
+#include <vector>
 
 /**
  * \brief Macro to suppress unused variable warnings
@@ -137,6 +138,8 @@ namespace iDynTree
      */
     bool checkDoublesAreEqual(const double & val1, const double & val2, double tol = DEFAULT_TOL);
 
+    void addNonZeroIfNotPresent(size_t newRow, size_t newCol, std::vector<size_t>& nonZerosRows, std::vector<size_t>& nonZerosCols);
+
 }
 
 #endif
diff --git a/src/core/src/Utils.cpp b/src/core/src/Utils.cpp
index cc3dcb3fd62..9a001e3edfa 100644
--- a/src/core/src/Utils.cpp
+++ b/src/core/src/Utils.cpp
@@ -101,5 +101,16 @@ namespace iDynTree
         return (std::fabs(val1-val2) < tol);
     }
 
+    void addNonZeroIfNotPresent(size_t newRow, size_t newCol, std::vector<size_t> &nonZerosRows, std::vector<size_t> &nonZerosCols)
+    {
+        for (size_t i = 0; i < nonZerosRows.size(); ++i) {
+            if ((newRow == nonZerosRows[i]) && (newCol == nonZerosCols[i])) {
+                return;
+            }
+        }
+        nonZerosRows.push_back(newRow);
+        nonZerosCols.push_back(newCol);
+    }
+
 
 }
diff --git a/src/optimalcontrol/include/iDynTree/Constraint.h b/src/optimalcontrol/include/iDynTree/Constraint.h
index 6bb342116f9..fc3c7073db0 100644
--- a/src/optimalcontrol/include/iDynTree/Constraint.h
+++ b/src/optimalcontrol/include/iDynTree/Constraint.h
@@ -162,6 +162,26 @@ namespace iDynTree {
              */
             virtual size_t expectedControlSpaceSize() const;
 
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, stateDimension) respectively.
+             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
+             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool constraintJacobianWRTStateSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
+             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
+             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool constraintJacobianWRTControlSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+
 
         private:
             size_t m_constraintSize;
diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index b7bbc3098d7..0964b00c9c1 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -96,6 +96,8 @@ namespace iDynTree {
 
             /**
              * @brief Remove a previously added constraint.
+             *
+             * Note: the sparsity pattern is not updated.
              * @param[in] name The name of the constraint that has to be removed
              * @return True if successfull. Possible causes of failure: a constraint does not exist with the specified name.
              */
@@ -189,6 +191,22 @@ namespace iDynTree {
                                               const VectorDynSize& control,
                                               MatrixDynSize& jacobian);
 
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
+             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
+             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            bool constraintJacobianWRTStateSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) const;
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
+             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
+             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            bool constraintJacobianWRTControlSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) const;
+
             /**
              * @brief Flag returning true if the group is an "AnyTime" group.
              * An "AnyTime" group contains only one constraint which is always enabled. It corresponds to a simple constraint.
diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index 862f18b4e4e..bd647775258 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -146,7 +146,7 @@ namespace optimalcontrol {
          * By default it return false;
          *
          * @param[in] state The state value at which computing the partial derivative.
-         * @param[in] control The control value at which computing the partial derivative.
+         * @param[in] time The time at which computing the partial derivative.
          * @param[out] dynamicsDerivative The output derivative. It has to be a square matrix with dimension equal to the state size.
          * @return True if successful, false otherwise (or if not implemented).
          */
@@ -161,7 +161,7 @@ namespace optimalcontrol {
          * By default it return false;
          *
          * @param[in] state The state value at which computing the partial derivative.
-         * @param[in] control The control value at which computing the partial derivative.
+         * @param[in] time The time at which computing the partial derivative.
          * @param[out] dynamicsDerivative The output derivative. It has to be a matrix with number of rows equal to the state size and number of columns equal to the control size.
          * @return True if successful, false otherwise (or if not implemented).
          */
@@ -169,6 +169,23 @@ namespace optimalcontrol {
                                                     double time,
                                                     MatrixDynSize& dynamicsDerivative);
 
+
+        /**
+         * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
+         * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
+         * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+         * @return true if the sparsity is available. False otherwise.
+         */
+        virtual bool dynamicsStateFirstDerivativeSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+
+        /**
+         * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
+         * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
+         * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+         * @return true if the sparsity is available. False otherwise.
+         */
+        virtual bool dynamicsControlFirstDerivativeSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+
     private:
         size_t m_stateSize;
         size_t m_controlSize;
diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index 5d5cd7dafab..5a25fa9f186 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -206,7 +206,8 @@ namespace optimalcontrol {
                 /**
                  * @brief Evaluate the collocation constraint.
                  *
-                 * In some optimal control solvers, like the MultipleShootingSolver, we need to discretize the dynamical system associated to the optimal control problem. This function evaluates the discretization error according to specified integration method. The Integrator, when integrating a dynamical system, is performing the following discretization:
+                 * In some optimal control solvers, like the MultipleShootingSolver, we need to discretize the dynamical system associated to the optimal control problem.
+                 * This function evaluates the discretization error according to specified integration method. The Integrator, when integrating a dynamical system, is performing the following discretization:
                  * \f[
                  * x_{k+1} = \phi(x_k, x_{k+1}, u_k, u_{k+1}, t)
                  * \f]
@@ -233,6 +234,16 @@ namespace optimalcontrol {
                                                                    std::vector<MatrixDynSize>& stateJacobianValues,
                                                                    std::vector<MatrixDynSize>& controlJacobianValues);
 
+                typedef struct {
+                    std::vector<size_t> nonZeroElementRows;
+                    std::vector<size_t> nonZeroElementColumns;
+                } CollocationSparsityVectors;
+
+                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity);
+
+                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity);
+
+
                 const IntegratorInfo& info() const;
 
             protected:
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
index 786cfc41df1..8f6f2566ec4 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
@@ -35,6 +35,10 @@ namespace iDynTree {
 
                 VectorDynSize m_computationBuffer;
                 MatrixDynSize m_stateJacBuffer, m_controlJacBuffer, m_identity, m_zeroBuffer;
+                bool m_hasStateSparsity = true;
+                bool m_hasControlSparsity = true;
+                std::vector<CollocationSparsityVectors> m_stateJacobianSparsity;
+                std::vector<CollocationSparsityVectors> m_controlJacobianSparsity;
 
                 bool allocateBuffers() override;
 
@@ -57,6 +61,10 @@ namespace iDynTree {
                                                            std::vector<MatrixDynSize> &stateJacobianValues,
                                                            std::vector<MatrixDynSize> &controlJacobianValues) override;
 
+                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity) override;
+
+                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity) override;
+
             };
         }
     }
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
index 2f625b93779..e43086ff278 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -35,10 +35,14 @@ namespace iDynTree {
 
                 VectorDynSize m_computationBuffer, m_computationBuffer2;
                 MatrixDynSize m_identity, m_stateJacBuffer, m_controlJacBuffer;
+                bool m_hasStateSparsity = true;
+                bool m_hasControlSparsity = true;
+                std::vector<CollocationSparsityVectors> m_stateJacobianSparsity;
+                std::vector<CollocationSparsityVectors> m_controlJacobianSparsity;
 
-                bool allocateBuffers() override;
+                virtual bool allocateBuffers() override;
 
-                bool oneStepIntegration(double t0, double dT, const VectorDynSize& x0, VectorDynSize& x) override;
+                virtual bool oneStepIntegration(double t0, double dT, const VectorDynSize& x0, VectorDynSize& x) override;
 
             public:
 
@@ -48,14 +52,18 @@ namespace iDynTree {
 
                 virtual ~ImplicitTrapezoidal() override;
 
-                bool evaluateCollocationConstraint(double time, const std::vector<VectorDynSize> &collocationPoints,
-                                                   const std::vector<VectorDynSize> &controlInputs, double dT,
-                                                   VectorDynSize &constraintValue) override;
-
-                bool evaluateCollocationConstraintJacobian(double time, const std::vector<VectorDynSize> &collocationPoints,
+                virtual bool evaluateCollocationConstraint(double time, const std::vector<VectorDynSize> &collocationPoints,
                                                            const std::vector<VectorDynSize> &controlInputs, double dT,
-                                                           std::vector<MatrixDynSize> &stateJacobianValues,
-                                                           std::vector<MatrixDynSize> &controlJacobianValues) override;
+                                                           VectorDynSize &constraintValue) override;
+
+                virtual bool evaluateCollocationConstraintJacobian(double time, const std::vector<VectorDynSize> &collocationPoints,
+                                                                   const std::vector<VectorDynSize> &controlInputs, double dT,
+                                                                   std::vector<MatrixDynSize> &stateJacobianValues,
+                                                                   std::vector<MatrixDynSize> &controlJacobianValues) override;
+
+                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity) override;
+
+                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity) override;
 
             };
 
diff --git a/src/optimalcontrol/src/Constraint.cpp b/src/optimalcontrol/src/Constraint.cpp
index c3f9945687c..a6acb10bf17 100644
--- a/src/optimalcontrol/src/Constraint.cpp
+++ b/src/optimalcontrol/src/Constraint.cpp
@@ -133,5 +133,15 @@ namespace iDynTree {
             return 0;
         }
 
+        bool Constraint::constraintJacobianWRTStateSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        {
+            return false;
+        }
+
+        bool Constraint::constraintJacobianWRTControlSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        {
+            return false;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index 3023a629152..8d7332ee1a4 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -48,10 +48,15 @@ namespace optimalcontrol {
         public:
             GroupOfConstraintsMap group;
             std::vector<TimedConstraint_ptr> orderedIntervals;
+            std::vector<size_t> nonZeroStateRows, nonZeroStateCols;
+            std::vector<size_t> nonZeroControlRows, nonZeroControlCols;
             std::string name;
             unsigned int maxConstraintSize;
             std::vector<TimeRange> timeRanges;
             bool isLinearGroup = true;
+            bool stateSparsityProvided = true;
+            bool controlSparsityProvided = true;
+
 
             std::vector<TimedConstraint_ptr>::reverse_iterator findActiveConstraint(double time){
                 return std::find_if(orderedIntervals.rbegin(),
@@ -118,6 +123,41 @@ namespace optimalcontrol {
                 orderedIntervals.push_back(result.first->second); //register the time range in order to have the constraints ordered by init time. result.first->second is the TimedConstraint_ptr of the newly inserted TimedConstraint.
                 std::sort(orderedIntervals.begin(), orderedIntervals.end(), [](const TimedConstraint_ptr&a, const TimedConstraint_ptr&b) { return a->timeRange < b->timeRange;}); //reorder the vector
 
+                std::vector<size_t> newConstraintNNZRowsState, newConstraintNNZRowsControl;
+                std::vector<size_t> newConstraintNNZColsState, newConstraintNNZColsControl;
+
+                if (stateSparsityProvided
+                        && constraint->constraintJacobianWRTStateSparsity(newConstraintNNZRowsState, newConstraintNNZColsState)) {
+
+                    if (newConstraintNNZRowsState.size() != newConstraintNNZColsState.size()) {
+                        reportError("ConstraintsGroup", "addConstraint", "The state sparsity is provided but the dimension of the two vectors do not match.");
+                        return false;
+                    }
+
+                    for (size_t i = 0; i < newConstraintNNZRowsState.size(); ++i) {
+                        addNonZeroIfNotPresent(newConstraintNNZRowsState[i], newConstraintNNZColsState[i], nonZeroStateRows, nonZeroStateCols);
+                    }
+
+                } else {
+                    stateSparsityProvided = false;
+                }
+
+                if (controlSparsityProvided
+                        && constraint->constraintJacobianWRTControlSparsity(newConstraintNNZRowsControl, newConstraintNNZColsControl)) {
+
+                    if (newConstraintNNZRowsControl.size() != newConstraintNNZColsControl.size()) {
+                        reportError("ConstraintsGroup", "addConstraint", "The control sparsity is provided but the dimension of the two vectors do not match.");
+                        return false;
+                    }
+
+                    for (size_t i = 0; i < newConstraintNNZRowsControl.size(); ++i) {
+                        addNonZeroIfNotPresent(newConstraintNNZRowsControl[i], newConstraintNNZColsControl[i], nonZeroControlRows, nonZeroControlCols);
+                    }
+
+                } else {
+                    controlSparsityProvided = false;
+                }
+
                 return true;
             }
 
@@ -404,7 +444,7 @@ namespace optimalcontrol {
             if (constraintIterator->get()->constraint->constraintSize() < m_pimpl->maxConstraintSize) {
                 toEigen(jacobian).block(0, 0, constraintIterator->get()->stateJacobianBuffer.rows(), state.size()) =
                         toEigen(constraintIterator->get()->stateJacobianBuffer);
-                int nMissing = m_pimpl->maxConstraintSize - constraintIterator->get()->constraint->constraintSize();
+                unsigned int nMissing = m_pimpl->maxConstraintSize - static_cast<unsigned int>(constraintIterator->get()->constraint->constraintSize());
                 toEigen(jacobian).block(constraintIterator->get()->stateJacobianBuffer.rows(), 0, nMissing, state.size()).setZero();
             } else {
                 jacobian = constraintIterator->get()->stateJacobianBuffer;
@@ -479,7 +519,7 @@ namespace optimalcontrol {
             if (constraintIterator->get()->constraint->constraintSize() < m_pimpl->maxConstraintSize) {
                 toEigen(jacobian).block(0, 0, constraintIterator->get()->controlJacobianBuffer.rows(), state.size()) =
                         toEigen(constraintIterator->get()->controlJacobianBuffer);
-                int nMissing = m_pimpl->maxConstraintSize - constraintIterator->get()->constraint->constraintSize();
+                unsigned int nMissing = m_pimpl->maxConstraintSize - static_cast<unsigned int>(constraintIterator->get()->constraint->constraintSize());
                 toEigen(jacobian).block(constraintIterator->get()->controlJacobianBuffer.rows(), 0, nMissing, state.size()).setZero();
             } else {
                 jacobian = constraintIterator->get()->controlJacobianBuffer;
@@ -488,6 +528,30 @@ namespace optimalcontrol {
             return true;
         }
 
+        bool ConstraintsGroup::constraintJacobianWRTStateSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns) const
+        {
+            if (!(m_pimpl->stateSparsityProvided)) {
+                return false;
+            }
+
+            nonZeroElementRows = m_pimpl->nonZeroStateRows;
+            nonZeroElementColumns = m_pimpl->nonZeroStateCols;
+
+            return true;
+        }
+
+        bool ConstraintsGroup::constraintJacobianWRTControlSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns) const
+        {
+            if (!(m_pimpl->controlSparsityProvided)) {
+                return false;
+            }
+
+            nonZeroElementRows = m_pimpl->nonZeroControlRows;
+            nonZeroElementColumns = m_pimpl->nonZeroControlCols;
+
+            return true;
+        }
+
         bool ConstraintsGroup::isAnyTimeGroup()
         {
             if ((numberOfConstraints() == 1) && (m_pimpl->group.begin()->second.get()->timeRange == TimeRange::AnyTime())) {
@@ -498,7 +562,7 @@ namespace optimalcontrol {
 
         unsigned int ConstraintsGroup::numberOfConstraints() const
         {
-            return m_pimpl->group.size();
+            return static_cast<unsigned int>(m_pimpl->group.size());
         }
 
         const std::vector<std::string> ConstraintsGroup::listConstraints() const
diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp
index e03a2356770..cd30256778b 100644
--- a/src/optimalcontrol/src/DynamicalSystem.cpp
+++ b/src/optimalcontrol/src/DynamicalSystem.cpp
@@ -90,6 +90,16 @@ namespace iDynTree {
                                                              MatrixDynSize& dynamicsDerivative)
         { return false; }
 
+        bool DynamicalSystem::dynamicsStateFirstDerivativeSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        {
+            return false;
+        }
+
+        bool DynamicalSystem::dynamicsControlFirstDerivativeSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        {
+            return false;
+        }
+
 //        bool DynamicalSystem::setController(std::shared_ptr<Controller> controllerPointer){
 //            if (controllerPointer->controlSpaceSize() != m_controlSize){
 //                reportError("DynamicalSystem", "setController", "The controller dimension is not coherent with the controlSpaceSize.");
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index ae5817d7314..fe38d2830d2 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -45,6 +45,29 @@ namespace iDynTree {
                 m_zeroBuffer.resize(nx,nu);
                 m_zeroBuffer.zero();
 
+                m_stateJacobianSparsity.resize(2);
+                m_controlJacobianSparsity.resize(2);
+
+                if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns)) {
+                    m_stateJacobianSparsity[1].nonZeroElementRows.clear();
+                    m_stateJacobianSparsity[1].nonZeroElementColumns.clear();
+
+                    for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
+                        addNonZeroIfNotPresent(i, i, m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns);
+                        m_stateJacobianSparsity[1].nonZeroElementRows.push_back(i);
+                        m_stateJacobianSparsity[1].nonZeroElementColumns.push_back(i);
+                    }
+                } else {
+                    m_hasStateSparsity = false;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0].nonZeroElementRows, m_controlJacobianSparsity[0].nonZeroElementColumns)) {
+                    m_controlJacobianSparsity[1].nonZeroElementRows.clear();
+                    m_controlJacobianSparsity[1].nonZeroElementColumns.clear();
+                } else {
+                    m_hasControlSparsity = false;
+                }
+
                 return true;
             }
 
@@ -218,6 +241,26 @@ namespace iDynTree {
 
                 return true;
             }
+
+            bool ForwardEuler::getCollocationConstraintJacobianStateSparsity(std::vector<Integrator::CollocationSparsityVectors> &stateJacobianSparsity)
+            {
+                if (!m_hasStateSparsity) {
+                    return false;
+                }
+
+                stateJacobianSparsity = m_stateJacobianSparsity;
+                return true;
+            }
+
+            bool ForwardEuler::getCollocationConstraintJacobianControlSparsity(std::vector<Integrator::CollocationSparsityVectors> &controlJacobianSparsity)
+            {
+                if (!m_hasControlSparsity) {
+                    return false;
+                }
+
+                controlJacobianSparsity = m_controlJacobianSparsity;
+                return true;
+            }
         }
     }
 }
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index 9f1a3807d93..d40971a5142 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -33,17 +33,40 @@ namespace iDynTree {
                     return false;
                 }
 
-                m_computationBuffer.resize(m_dynamicalSystem_ptr->stateSpaceSize());
-                m_computationBuffer2.resize(m_dynamicalSystem_ptr->stateSpaceSize());
+                m_computationBuffer.resize(static_cast<unsigned int>(m_dynamicalSystem_ptr->stateSpaceSize()));
+                m_computationBuffer2.resize(static_cast<unsigned int>(m_dynamicalSystem_ptr->stateSpaceSize()));
 
-                size_t nx = m_dynamicalSystem_ptr->stateSpaceSize();
-                size_t nu = m_dynamicalSystem_ptr->controlSpaceSize();
+                unsigned int nx = static_cast<unsigned int>(m_dynamicalSystem_ptr->stateSpaceSize());
+                unsigned int nu = static_cast<unsigned int>(m_dynamicalSystem_ptr->controlSpaceSize());
 
                 m_identity.resize(nx, nx);
                 toEigen(m_identity) = iDynTreeEigenMatrix::Identity(nx, nx);
 
                 m_stateJacBuffer.resize(nx, nx);
                 m_controlJacBuffer.resize(nx,nu);
+
+                m_stateJacobianSparsity.resize(2);
+                m_controlJacobianSparsity.resize(2);
+
+                if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns)) {
+
+                    for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
+                        addNonZeroIfNotPresent(i, i, m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns);
+                    }
+                    m_stateJacobianSparsity[1] = m_stateJacobianSparsity[0];
+
+                } else {
+                    m_hasStateSparsity = false;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0].nonZeroElementRows, m_controlJacobianSparsity[0].nonZeroElementColumns)) {
+
+                    m_controlJacobianSparsity[1] = m_controlJacobianSparsity[0];
+
+                } else {
+                    m_hasControlSparsity = false;
+                }
+
                 return true;
             }
 
@@ -228,6 +251,25 @@ namespace iDynTree {
 
             }
 
+            bool ImplicitTrapezoidal::getCollocationConstraintJacobianStateSparsity(std::vector<Integrator::CollocationSparsityVectors> &stateJacobianSparsity)
+            {
+                if (!m_hasStateSparsity) {
+                    return false;
+                }
+
+                stateJacobianSparsity = m_stateJacobianSparsity;
+                return true;
+            }
+
+            bool ImplicitTrapezoidal::getCollocationConstraintJacobianControlSparsity(std::vector<Integrator::CollocationSparsityVectors> &controlJacobianSparsity)
+            {
+                if (!m_hasControlSparsity) {
+                    return false;
+                }
+
+                controlJacobianSparsity = m_controlJacobianSparsity;
+                return true;
+            }
 
         }
     }
diff --git a/src/optimalcontrol/src/Integrator.cpp b/src/optimalcontrol/src/Integrator.cpp
index 5ae95ebb654..221382b63a3 100644
--- a/src/optimalcontrol/src/Integrator.cpp
+++ b/src/optimalcontrol/src/Integrator.cpp
@@ -129,6 +129,16 @@ namespace iDynTree {
                 return false;
             }
 
+            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<Integrator::CollocationSparsityVectors> &stateJacobianSparsity)
+            {
+                return false;
+            }
+
+            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<Integrator::CollocationSparsityVectors> &controlJacobianSparsity)
+            {
+                return false;
+            }
+
             const IntegratorInfo &Integrator::info() const
             {
                 return m_info;

From 21826befc707fb1c0fb704eccb62f2d70a2e1f06 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 17 Oct 2018 18:37:37 +0200
Subject: [PATCH 073/194] Using finer sparsity in OptimalControlProblem and
 MultipleShootingSolver.

---
 .../include/iDynTree/ConstraintsGroup.h       |   2 +-
 .../include/iDynTree/Integrator.h             |  10 +-
 .../iDynTree/Integrators/ForwardEuler.h       |   4 +-
 .../Integrators/ImplicitTrapezoidal.h         |   4 +-
 .../include/iDynTree/OptimalControlProblem.h  |   4 +
 src/optimalcontrol/src/ForwardEuler.cpp       |  12 +-
 .../src/ImplicitTrapezoidal.cpp               |  11 +-
 src/optimalcontrol/src/Integrator.cpp         |   4 +-
 .../src/MultipleShootingSolver.cpp            | 128 ++++++++++--
 .../src/OptimalControlProblem.cpp             | 186 +++++++++++++++---
 10 files changed, 304 insertions(+), 61 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index 0964b00c9c1..92a2766c061 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -80,7 +80,7 @@ namespace iDynTree {
 
             /**
              * @brief Add a linear constraint to the group
-             * @param[in] constraint Shared pointer to the user defined constraint.
+             * @param[in] linearConstraint Shared pointer to a linear constraint.
              * @param[in] timeRange Time range in which the constraint will be enabled.
              * @return True if successfull. Posible causes of failures are: empty pointer, dimension bigger than maxConstraintSize, invalid TimeRange.
              */
diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index 5a25fa9f186..b46c6e80c28 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -52,6 +52,11 @@ namespace optimalcontrol {
                 double time;
             };
 
+            typedef struct {
+                std::vector<size_t> nonZeroElementRows;
+                std::vector<size_t> nonZeroElementColumns;
+            } CollocationSparsityVectors;
+
 
             class IntegratorInfoData {
             protected:
@@ -234,11 +239,6 @@ namespace optimalcontrol {
                                                                    std::vector<MatrixDynSize>& stateJacobianValues,
                                                                    std::vector<MatrixDynSize>& controlJacobianValues);
 
-                typedef struct {
-                    std::vector<size_t> nonZeroElementRows;
-                    std::vector<size_t> nonZeroElementColumns;
-                } CollocationSparsityVectors;
-
                 virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity);
 
                 virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity);
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
index 8f6f2566ec4..9fb64bdd37d 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
@@ -35,8 +35,8 @@ namespace iDynTree {
 
                 VectorDynSize m_computationBuffer;
                 MatrixDynSize m_stateJacBuffer, m_controlJacBuffer, m_identity, m_zeroBuffer;
-                bool m_hasStateSparsity = true;
-                bool m_hasControlSparsity = true;
+                bool m_hasStateSparsity = false;
+                bool m_hasControlSparsity = false;
                 std::vector<CollocationSparsityVectors> m_stateJacobianSparsity;
                 std::vector<CollocationSparsityVectors> m_controlJacobianSparsity;
 
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
index e43086ff278..376e2ea4d7c 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -35,8 +35,8 @@ namespace iDynTree {
 
                 VectorDynSize m_computationBuffer, m_computationBuffer2;
                 MatrixDynSize m_identity, m_stateJacBuffer, m_controlJacBuffer;
-                bool m_hasStateSparsity = true;
-                bool m_hasControlSparsity = true;
+                bool m_hasStateSparsity = false;
+                bool m_hasControlSparsity = false;
                 std::vector<CollocationSparsityVectors> m_stateJacobianSparsity;
                 std::vector<CollocationSparsityVectors> m_controlJacobianSparsity;
 
diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index 06622bfe497..08bc8b3a97c 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -225,6 +225,10 @@ namespace iDynTree {
                                                const VectorDynSize& control,
                                                MatrixDynSize& jacobian);
 
+            bool constraintJacobianWRTStateSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+
+            bool constraintJacobianWRTControlSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+
         private:
             class OptimalControlProblemPimpl;
             OptimalControlProblemPimpl* m_pimpl;
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index fe38d2830d2..9f6fdc537d6 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -57,15 +57,15 @@ namespace iDynTree {
                         m_stateJacobianSparsity[1].nonZeroElementRows.push_back(i);
                         m_stateJacobianSparsity[1].nonZeroElementColumns.push_back(i);
                     }
-                } else {
-                    m_hasStateSparsity = false;
+
+                    m_hasStateSparsity = true;
                 }
 
                 if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0].nonZeroElementRows, m_controlJacobianSparsity[0].nonZeroElementColumns)) {
                     m_controlJacobianSparsity[1].nonZeroElementRows.clear();
                     m_controlJacobianSparsity[1].nonZeroElementColumns.clear();
-                } else {
-                    m_hasControlSparsity = false;
+
+                    m_hasControlSparsity = true;
                 }
 
                 return true;
@@ -242,7 +242,7 @@ namespace iDynTree {
                 return true;
             }
 
-            bool ForwardEuler::getCollocationConstraintJacobianStateSparsity(std::vector<Integrator::CollocationSparsityVectors> &stateJacobianSparsity)
+            bool ForwardEuler::getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors> &stateJacobianSparsity)
             {
                 if (!m_hasStateSparsity) {
                     return false;
@@ -252,7 +252,7 @@ namespace iDynTree {
                 return true;
             }
 
-            bool ForwardEuler::getCollocationConstraintJacobianControlSparsity(std::vector<Integrator::CollocationSparsityVectors> &controlJacobianSparsity)
+            bool ForwardEuler::getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors> &controlJacobianSparsity)
             {
                 if (!m_hasControlSparsity) {
                     return false;
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index d40971a5142..2f58f05b442 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -55,16 +55,15 @@ namespace iDynTree {
                     }
                     m_stateJacobianSparsity[1] = m_stateJacobianSparsity[0];
 
-                } else {
-                    m_hasStateSparsity = false;
+                    m_hasStateSparsity = true;
                 }
 
                 if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0].nonZeroElementRows, m_controlJacobianSparsity[0].nonZeroElementColumns)) {
 
                     m_controlJacobianSparsity[1] = m_controlJacobianSparsity[0];
 
-                } else {
-                    m_hasControlSparsity = false;
+                    m_hasControlSparsity = true;
+
                 }
 
                 return true;
@@ -251,7 +250,7 @@ namespace iDynTree {
 
             }
 
-            bool ImplicitTrapezoidal::getCollocationConstraintJacobianStateSparsity(std::vector<Integrator::CollocationSparsityVectors> &stateJacobianSparsity)
+            bool ImplicitTrapezoidal::getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors> &stateJacobianSparsity)
             {
                 if (!m_hasStateSparsity) {
                     return false;
@@ -261,7 +260,7 @@ namespace iDynTree {
                 return true;
             }
 
-            bool ImplicitTrapezoidal::getCollocationConstraintJacobianControlSparsity(std::vector<Integrator::CollocationSparsityVectors> &controlJacobianSparsity)
+            bool ImplicitTrapezoidal::getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors> &controlJacobianSparsity)
             {
                 if (!m_hasControlSparsity) {
                     return false;
diff --git a/src/optimalcontrol/src/Integrator.cpp b/src/optimalcontrol/src/Integrator.cpp
index 221382b63a3..bb85870a8b7 100644
--- a/src/optimalcontrol/src/Integrator.cpp
+++ b/src/optimalcontrol/src/Integrator.cpp
@@ -129,12 +129,12 @@ namespace iDynTree {
                 return false;
             }
 
-            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<Integrator::CollocationSparsityVectors> &stateJacobianSparsity)
+            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors> &stateJacobianSparsity)
             {
                 return false;
             }
 
-            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<Integrator::CollocationSparsityVectors> &controlJacobianSparsity)
+            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors> &controlJacobianSparsity)
             {
                 return false;
             }
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 80da8a227d3..01fcc791606 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -97,6 +97,9 @@ namespace iDynTree {
             double m_minStepSize, m_maxStepSize, m_controlPeriod;
             size_t m_nx, m_nu, m_numberOfVariables, m_constraintsPerInstant, m_numberOfConstraints;
             std::vector<size_t> m_jacobianNZRows, m_jacobianNZCols, m_hessianNZRows, m_hessianNZCols;
+            std::vector<size_t> m_ocStateNZRows, m_ocStateNZCols, m_ocControlNZRows, m_ocControlNZCols;
+            std::vector<integrators::CollocationSparsityVectors> m_collocationStateNZ, m_collocationControlNZ;
+            integrators::CollocationSparsityVectors m_mergedCollocationControlNZ;
             size_t m_jacobianNonZeros, m_hessianNonZeros;
             double m_plusInfinity, m_minusInfinity;
             VectorDynSize m_constraintsLowerBound, m_constraintsUpperBound;
@@ -184,6 +187,14 @@ namespace iDynTree {
                 }
             }
 
+            void addJacobianBlock(size_t initRow, size_t initCol, const std::vector<size_t> &nonZeroRows, const std::vector<size_t> &nonZeroCols){
+                for (size_t i = 0; i < nonZeroRows.size(); ++i) {
+                    addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + nonZeroRows[i]);
+                    addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + nonZeroCols[i]);
+                    m_jacobianNonZeros++;
+                }
+            }
+
             void addIdentityJacobianBlock(size_t initRow, size_t initCol, size_t dimension){
                 for (size_t i = 0; i < dimension; ++i){
                     addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + i);
@@ -202,6 +213,36 @@ namespace iDynTree {
                 }
             }
 
+            void mergeSparsityVectors(const std::vector<integrators::CollocationSparsityVectors>& original, integrators::CollocationSparsityVectors& merged) {
+                const std::vector<size_t>& firstRows = original[0].nonZeroElementRows;
+                const std::vector<size_t>& firstCols = original[0].nonZeroElementColumns;
+                const std::vector<size_t>& secondRows = original[1].nonZeroElementRows;
+                const std::vector<size_t>& secondCols = original[1].nonZeroElementColumns;
+                size_t mergedNonZeros = 0;
+                for (size_t i = 0; i < firstRows.size(); ++i) {
+                    addNonZero(merged.nonZeroElementRows, mergedNonZeros, firstRows[i]);
+                    addNonZero(merged.nonZeroElementColumns, mergedNonZeros, firstCols[i]);
+                    mergedNonZeros++;
+                }
+
+                for (size_t j = 0; j < secondRows.size(); ++j) {
+                    bool duplicate = false;
+                    size_t i = 0;
+                    while (!duplicate && (i < firstRows.size())) {
+                        if ((secondRows[j] == firstRows[i]) && (secondCols[j] == firstCols[i])) {
+                            duplicate = true;
+                        }
+                        ++i;
+                    }
+
+                    if (!duplicate) {
+                        addNonZero(merged.nonZeroElementRows, mergedNonZeros, secondRows[j]);
+                        addNonZero(merged.nonZeroElementColumns, mergedNonZeros, secondCols[j]);
+                        mergedNonZeros++;
+                    }
+                }
+            }
+
             void allocateBuffers(){
                 if (m_stateBuffer.size() != m_nx) {
                     m_stateBuffer.resize(static_cast<unsigned int>(m_nx));
@@ -972,6 +1013,23 @@ namespace iDynTree {
                 Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(m_constraintsLowerBound);
                 Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(m_constraintsUpperBound);
 
+                bool ocHasStateSparsisty = m_ocproblem->constraintJacobianWRTStateSparsity(m_ocStateNZRows, m_ocStateNZCols);
+                if (!ocHasStateSparsisty) {
+                    reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve state sparsity of optimal control problem constraints. Assuming dense matrix.");
+                }
+
+                bool ocHasControlSparsisty = m_ocproblem->constraintJacobianWRTControlSparsity(m_ocControlNZRows, m_ocControlNZCols);
+                if (!ocHasControlSparsisty) {
+                    reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve control sparsity of optimal control problem constraints. Assuming dense matrix.");
+                }
+
+                bool systemHasStateSparsity = m_integrator->getCollocationConstraintJacobianStateSparsity(m_collocationStateNZ);
+                bool systemHasControlSparsity = m_integrator->getCollocationConstraintJacobianControlSparsity(m_collocationControlNZ);
+
+                if (systemHasControlSparsity) {
+                    mergeSparsityVectors(m_collocationControlNZ, m_mergedCollocationControlNZ);
+                }
+
                 resetNonZerosCount();
 
                 std::vector<MeshPoint>::iterator mesh = m_meshPoints.begin(), previousControlMesh = mesh;
@@ -1016,7 +1074,16 @@ namespace iDynTree {
                         }
 
                         //Saving the jacobian structure due to the constraints
-                        addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        if (ocHasStateSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateNZRows, m_ocStateNZCols);
+                        } else {
+                            addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
+                        }
+                        if (ocHasControlSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
+                        } else {
+                            addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        }
                         constraintIndex += nc;
 
                         //Saving the hessian structure
@@ -1045,10 +1112,21 @@ namespace iDynTree {
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
 
                         //Saving the jacobian structure due to the dynamical constraints
-                        addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
-                        addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
-                        addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
-                        addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+                        if (systemHasControlSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_collocationControlNZ[1].nonZeroElementRows, m_collocationControlNZ[1].nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, mesh->previousControlIndex, m_collocationControlNZ[0].nonZeroElementRows, m_collocationControlNZ[0].nonZeroElementColumns);
+                        } else {
+                            addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
+                            addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
+                        }
+
+                        if (systemHasStateSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1].nonZeroElementRows, m_collocationStateNZ[1].nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0].nonZeroElementRows, m_collocationStateNZ[0].nonZeroElementColumns);
+                        } else {
+                            addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
+                            addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+                        }
 
                         constraintIndex += nx;
 
@@ -1076,8 +1154,16 @@ namespace iDynTree {
                         }
 
                         //Saving the jacobian structure due to the constraints
-                        addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
-                        addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        if (ocHasStateSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateNZRows, m_ocStateNZCols);
+                        } else {
+                            addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
+                        }
+                        if (ocHasControlSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
+                        } else {
+                            addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        }
                         constraintIndex += nc;
 
                         //Saving the hessian structure
@@ -1109,9 +1195,19 @@ namespace iDynTree {
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
 
                         //Saving the jacobian structure due to the dynamical constraints
-                        addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
-                        addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
-                        addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+                        if (systemHasControlSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_mergedCollocationControlNZ.nonZeroElementRows, m_mergedCollocationControlNZ.nonZeroElementColumns);
+                        } else {
+                            addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
+                        }
+
+                        if (systemHasStateSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1].nonZeroElementRows, m_collocationStateNZ[1].nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0].nonZeroElementRows, m_collocationStateNZ[0].nonZeroElementColumns);
+                        } else {
+                            addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
+                            addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
+                        }
                         constraintIndex += nx;
 
                         //Saving constraints bounds
@@ -1137,8 +1233,16 @@ namespace iDynTree {
                             }
                         }
 
-                        addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
-                        addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        if (ocHasStateSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateNZRows, m_ocStateNZCols);
+                        } else {
+                            addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
+                        }
+                        if (ocHasControlSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
+                        } else {
+                            addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
+                        }
                         constraintIndex += nc;
 
                         //Saving the hessian structure
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 2ad083261d8..5aa2cc27770 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -38,7 +38,6 @@
 #include <sstream>
 
 
-
 namespace iDynTree {
     namespace optimalcontrol {
 
@@ -46,6 +45,10 @@ namespace iDynTree {
             std::shared_ptr<ConstraintsGroup> group_ptr;
             VectorDynSize constraintsBuffer;
             MatrixDynSize stateJacobianBuffer, controlJacobianBuffer;
+            std::vector<size_t> nonZeroStateRows, nonZeroStateCols;
+            std::vector<size_t> nonZeroControlRows, nonZeroControlCols;
+            bool hasStateSparsity = false;
+            bool hasControlSparsity = false;
         } BufferedGroup;
 
         typedef std::shared_ptr<BufferedGroup> BufferedGroup_ptr;
@@ -73,6 +76,8 @@ namespace iDynTree {
             MatrixDynSize costStateHessianBuffer, costControlHessianBuffer, costMixedHessianBuffer;
             bool stateLowerBounded, stateUpperBounded, controlLowerBounded, controlUpperBounded;
             VectorDynSize stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
+            std::vector<size_t> nonZeroStateRows, nonZeroStateCols;
+            std::vector<size_t> nonZeroControlRows, nonZeroControlCols;
             std::vector<std::string> mayerCostnames;
             std::vector<TimeRange> constraintsTimeRanges, costTimeRanges;
             std::vector<size_t> linearConstraintIndeces;
@@ -135,7 +140,7 @@ namespace iDynTree {
             }
 
             CostsMap::iterator costIterator;
-            for (auto mayerCost : m_pimpl->mayerCostnames){
+            for (auto& mayerCost : m_pimpl->mayerCostnames){
                 if (mayerCost.size() > 0){
                     costIterator = m_pimpl->costs.find(mayerCost);
                     if (costIterator == m_pimpl->costs.end()){
@@ -208,6 +213,9 @@ namespace iDynTree {
                 newGroup->controlJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize()));
             }
 
+            newGroup->hasStateSparsity = groupOfConstraints->constraintJacobianWRTStateSparsity(newGroup->nonZeroStateRows, newGroup->nonZeroStateCols); //needed to allocate memory in advance
+            newGroup->hasControlSparsity = groupOfConstraints->constraintJacobianWRTControlSparsity(newGroup->nonZeroControlRows, newGroup->nonZeroControlCols); //needed to allocate memory in advance
+
             std::pair< ConstraintsGroupsMap::iterator, bool> groupResult;
             groupResult = m_pimpl->constraintsGroups.insert(std::pair< std::string, BufferedGroup_ptr>(groupOfConstraints->name(), newGroup));
 
@@ -217,6 +225,13 @@ namespace iDynTree {
                 reportError("OptimalControlProblem", "addGroupOfConstraints", errorMsg.str().c_str());
                 return false;
             }
+
+            m_pimpl->nonZeroStateRows.resize(m_pimpl->nonZeroStateRows.size() + newGroup->nonZeroStateRows.size()); //needed to allocate memory in advance
+            m_pimpl->nonZeroStateCols.resize(m_pimpl->nonZeroStateCols.size() + newGroup->nonZeroStateCols.size()); //needed to allocate memory in advance
+
+            m_pimpl->nonZeroControlRows.resize(m_pimpl->nonZeroControlRows.size() + newGroup->nonZeroControlRows.size()); //needed to allocate memory in advance
+            m_pimpl->nonZeroControlCols.resize(m_pimpl->nonZeroControlCols.size() + newGroup->nonZeroControlCols.size()); //needed to allocate memory in advance
+
             return true;
         }
 
@@ -316,7 +331,7 @@ namespace iDynTree {
         {
             unsigned int number = 0;
 
-            for(auto group: m_pimpl->constraintsGroups){
+            for(auto& group: m_pimpl->constraintsGroups){
                 number += group.second->group_ptr->numberOfConstraints();
             }
 
@@ -327,7 +342,7 @@ namespace iDynTree {
         {
             unsigned int number = 0;
 
-            for(auto group: m_pimpl->constraintsGroups){
+            for(auto& group: m_pimpl->constraintsGroups){
                 if (group.second->group_ptr->isLinearGroup()) {
                     number += group.second->group_ptr->constraintsDimension();
                 }
@@ -340,7 +355,7 @@ namespace iDynTree {
         {
             unsigned int dimension = 0;
 
-            for (auto group: m_pimpl->constraintsGroups){
+            for (auto& group: m_pimpl->constraintsGroups){
                 dimension += group.second->group_ptr->constraintsDimension();
             }
             return dimension;
@@ -351,7 +366,7 @@ namespace iDynTree {
             std::vector<std::string> output;
             std::vector<std::string> temp;
 
-            for(auto group: m_pimpl->constraintsGroups){
+            for(auto& group: m_pimpl->constraintsGroups){
                 temp =  group.second->group_ptr->listConstraints();
                 output.insert(output.end(), temp.begin(), temp.end());
             }
@@ -362,7 +377,7 @@ namespace iDynTree {
         {
             std::vector<std::string> output;
 
-            for(auto group: m_pimpl->constraintsGroups){
+            for(auto& group: m_pimpl->constraintsGroups){
                 output.insert(output.end(), group.second->group_ptr->numberOfConstraints(), group.second->group_ptr->name());
             }
             return output;
@@ -377,7 +392,7 @@ namespace iDynTree {
 
             size_t index = 0;
 
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 std::vector<TimeRange> &groupTimeRanges = group.second->group_ptr->getTimeRanges();
                 for (size_t i = 0; i < groupTimeRanges.size(); ++i){
                     m_pimpl->constraintsTimeRanges[index] = groupTimeRanges[i];
@@ -397,7 +412,7 @@ namespace iDynTree {
             size_t vectorIndex = 0, constraintIndex = 0;
 
             unsigned int nc = 0;
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 nc = group.second->group_ptr->constraintsDimension();
 
                 if (group.second->group_ptr->isLinearGroup()) {
@@ -567,7 +582,7 @@ namespace iDynTree {
 
         bool OptimalControlProblem::updateCostTimeRange(const std::string &name, const TimeRange &newTimeRange)
         {
-            for (auto mayerCost : m_pimpl->mayerCostnames)
+            for (auto& mayerCost : m_pimpl->mayerCostnames)
                 if (name == mayerCost){
                     std::ostringstream errorMsg;
                     errorMsg << "Cannot change the TimeRange of cost named " << name << " since it is a terminal cost." << std::endl;
@@ -611,7 +626,7 @@ namespace iDynTree {
             }
 
             size_t i = 0;
-            for (auto c : m_pimpl->costs){
+            for (auto& c : m_pimpl->costs){
                 m_pimpl->costTimeRanges[i] = c.second.timeRange;
                 ++i;
             }
@@ -620,7 +635,7 @@ namespace iDynTree {
 
         bool OptimalControlProblem::hasOnlyLinearCosts() const
         {
-            for (auto c : m_pimpl->costs) {
+            for (auto& c : m_pimpl->costs) {
                 if (!(c.second.isLinear)) {
                     return false;
                 }
@@ -630,7 +645,7 @@ namespace iDynTree {
 
         bool OptimalControlProblem::hasOnlyQuadraticCosts() const
         {
-            for (auto c : m_pimpl->costs) {
+            for (auto& c : m_pimpl->costs) {
                 if (!(c.second.isQuadratic)) {
                     return false;
                 }
@@ -752,7 +767,7 @@ namespace iDynTree {
         {
             costValue = 0;
             double addCost;
-            for(auto cost : m_pimpl->costs){
+            for(auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
                     if(!cost.second.cost->costEvaluation(time, state, control, addCost)){
                         std::ostringstream errorMsg;
@@ -779,7 +794,7 @@ namespace iDynTree {
 
             bool first = true;
 
-            for(auto cost : m_pimpl->costs){
+            for(auto&  cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
                     if(!cost.second.cost->costFirstPartialDerivativeWRTState(time, state, control, m_pimpl->costStateJacobianBuffer)){
                         std::ostringstream errorMsg;
@@ -816,7 +831,7 @@ namespace iDynTree {
 
             bool first = true;
 
-            for(auto cost : m_pimpl->costs){
+            for(auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
                     if (!cost.second.cost->costFirstPartialDerivativeWRTControl(time, state, control, m_pimpl->costControlJacobianBuffer)){
                         std::ostringstream errorMsg;
@@ -853,7 +868,7 @@ namespace iDynTree {
 
             bool first = true;
 
-            for (auto cost : m_pimpl->costs){
+            for (auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
                     if (!cost.second.cost->costSecondPartialDerivativeWRTState(time, state, control, m_pimpl->costStateHessianBuffer)){
                         std::ostringstream errorMsg;
@@ -891,7 +906,7 @@ namespace iDynTree {
 
             bool first = true;
 
-            for (auto cost : m_pimpl->costs){
+            for (auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
                     if (!cost.second.cost->costSecondPartialDerivativeWRTControl(time, state, control, m_pimpl->costControlHessianBuffer)){
                         std::ostringstream errorMsg;
@@ -930,7 +945,7 @@ namespace iDynTree {
 
             bool first = true;
 
-            for (auto cost : m_pimpl->costs){
+            for (auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
                     if (!cost.second.cost->costSecondPartialDerivativeWRTStateControl(time, state, control, m_pimpl->costMixedHessianBuffer)){
                         std::ostringstream errorMsg;
@@ -966,7 +981,7 @@ namespace iDynTree {
             Eigen::Map< Eigen::VectorXd > constraintsEvaluation = toEigen(constraintsValue);
             Eigen::Index offset = 0;
 
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 if (!(group.second->group_ptr->evaluateConstraints(time, state, control, group.second->constraintsBuffer))){
                     std::ostringstream errorMsg;
                     errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
@@ -990,7 +1005,7 @@ namespace iDynTree {
             Eigen::Map< Eigen::VectorXd > upperBoundMap = toEigen(upperBound);
             Eigen::Index offset = 0;
 
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 if (! group.second->group_ptr->getUpperBound(time, group.second->constraintsBuffer)){
                     toEigen(group.second->constraintsBuffer).setConstant(std::abs(infinity)); //if not upper bounded
                 }
@@ -1018,7 +1033,7 @@ namespace iDynTree {
             Eigen::Map< Eigen::VectorXd > lowerBoundMap = toEigen(lowerBound);
             Eigen::Index offset = 0;
 
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 if (! group.second->group_ptr->getLowerBound(time, group.second->constraintsBuffer)){
                     toEigen(group.second->constraintsBuffer).setConstant(-std::abs(infinity)); //if not lower bounded
                 }
@@ -1039,7 +1054,7 @@ namespace iDynTree {
 
         bool OptimalControlProblem::isFeasiblePoint(double time, const VectorDynSize &state, const VectorDynSize &control)
         {
-            for(auto group : m_pimpl->constraintsGroups){
+            for(auto& group : m_pimpl->constraintsGroups){
                 if(!(group.second->group_ptr->isFeasibilePoint(time, state, control))){
                     return false;
                 }
@@ -1057,7 +1072,7 @@ namespace iDynTree {
             iDynTreeEigenMatrixMap jacobianMap = toEigen(jacobian);
             Eigen::Index offset = 0;
 
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 if (!(group.second->group_ptr->constraintJacobianWRTState(time, state, control, group.second->stateJacobianBuffer))){
                     std::ostringstream errorMsg;
                     errorMsg << "Error while evaluating constraint group " << group.second->group_ptr->name() <<".";
@@ -1081,7 +1096,7 @@ namespace iDynTree {
             iDynTreeEigenMatrixMap jacobianMap = toEigen(jacobian);
             Eigen::Index offset = 0;
 
-            for (auto group : m_pimpl->constraintsGroups){
+            for (auto& group : m_pimpl->constraintsGroups){
                 if (! group.second->group_ptr->constraintJacobianWRTControl(time, state, control, group.second->controlJacobianBuffer)){
                     std::ostringstream errorMsg;
                     errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
@@ -1094,5 +1109,126 @@ namespace iDynTree {
             return true;
         }
 
+        bool OptimalControlProblem::constraintJacobianWRTStateSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        {
+            size_t nonZeroIndexState = 0;
+            size_t offset = 0;
+            for (auto& group : m_pimpl->constraintsGroups){
+
+                group.second->hasStateSparsity = group.second->group_ptr->constraintJacobianWRTStateSparsity(group.second->nonZeroStateRows, group.second->nonZeroStateCols);
+
+                if (group.second->hasStateSparsity) {
+
+                    if (m_pimpl->nonZeroStateRows.size() < (nonZeroIndexState + group.second->nonZeroStateRows.size())) {
+                        m_pimpl->nonZeroStateRows.resize(nonZeroIndexState + group.second->nonZeroStateRows.size());
+                         m_pimpl->nonZeroStateCols.resize(nonZeroIndexState + group.second->nonZeroStateRows.size());
+                    }
+
+                    for (size_t i = 0; i < group.second->nonZeroStateRows.size(); ++i) {
+                        m_pimpl->nonZeroStateRows[nonZeroIndexState] = group.second->nonZeroStateRows[i] + offset;
+                        m_pimpl->nonZeroStateCols[nonZeroIndexState] = group.second->nonZeroStateCols[i];
+                        nonZeroIndexState++;
+                    }
+
+                } else {
+
+                    if (m_pimpl->dynamicalSystem) {
+
+                        size_t rows = group.second->controlJacobianBuffer.rows();
+                        size_t cols = group.second->controlJacobianBuffer.cols();
+                        size_t nonZeros = rows * cols;
+
+                        if (m_pimpl->nonZeroStateRows.size() < (nonZeroIndexState + nonZeros)) {
+                            m_pimpl->nonZeroStateRows.resize(nonZeroIndexState + nonZeros);
+                             m_pimpl->nonZeroStateCols.resize(nonZeroIndexState + nonZeros);
+                        }
+
+                        for (size_t i = 0; i < rows; ++i) {
+                            for (size_t j = 0; j < cols; ++j) {
+                                m_pimpl->nonZeroStateRows[nonZeroIndexState] = i + offset;
+                                m_pimpl->nonZeroStateCols[nonZeroIndexState] = j;
+                                nonZeroIndexState++;
+                            }
+                        }
+                    } else {
+                        std::ostringstream errorMsg;
+                        errorMsg << "The group " << group.second->group_ptr->name() <<" has dense state jacobian and no dynamical system has been provided yet. Cannot determine the sparsity since the state dimension is unknown.";
+                        reportError("OptimalControlProblem", "constraintJacobianWRTStateSparsity", errorMsg.str().c_str());
+                        return false;
+                    }
+                }
+
+                offset += group.second->stateJacobianBuffer.rows();
+            }
+            m_pimpl->nonZeroStateRows.resize(nonZeroIndexState); //remove leftovers
+            m_pimpl->nonZeroStateCols.resize(nonZeroIndexState); //remove leftovers
+
+
+            nonZeroElementRows = m_pimpl->nonZeroStateRows;
+            nonZeroElementColumns = m_pimpl->nonZeroStateCols;
+
+            return true;
+        }
+
+        bool OptimalControlProblem::constraintJacobianWRTControlSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        {
+            size_t nonZeroIndexControl = 0;
+            size_t offset = 0;
+            for (auto& group : m_pimpl->constraintsGroups){
+
+                group.second->hasControlSparsity = group.second->group_ptr->constraintJacobianWRTControlSparsity(group.second->nonZeroControlRows, group.second->nonZeroControlCols);
+
+                if (group.second->hasControlSparsity) {
+
+                    if (m_pimpl->nonZeroControlRows.size() < (nonZeroIndexControl + group.second->nonZeroControlRows.size())) {
+                        m_pimpl->nonZeroControlRows.resize(nonZeroIndexControl + group.second->nonZeroControlRows.size());
+                         m_pimpl->nonZeroControlCols.resize(nonZeroIndexControl + group.second->nonZeroControlRows.size());
+                    }
+
+                    for (size_t i = 0; i < group.second->nonZeroControlRows.size(); ++i) {
+                        m_pimpl->nonZeroControlRows[nonZeroIndexControl] = group.second->nonZeroControlRows[i] + offset;
+                        m_pimpl->nonZeroControlCols[nonZeroIndexControl] = group.second->nonZeroControlCols[i];
+                        nonZeroIndexControl++;
+                    }
+
+                } else {
+
+                    if (m_pimpl->dynamicalSystem) {
+
+                        size_t rows = group.second->controlJacobianBuffer.rows();
+                        size_t cols = group.second->controlJacobianBuffer.cols();
+                        size_t nonZeros = rows * cols;
+
+                        if (m_pimpl->nonZeroControlRows.size() < (nonZeroIndexControl + nonZeros)) {
+                            m_pimpl->nonZeroControlRows.resize(nonZeroIndexControl + nonZeros);
+                             m_pimpl->nonZeroControlCols.resize(nonZeroIndexControl + nonZeros);
+                        }
+
+                        for (size_t i = 0; i < rows; ++i) {
+                            for (size_t j = 0; j < cols; ++j) {
+                                m_pimpl->nonZeroControlRows[nonZeroIndexControl] = i + offset;
+                                m_pimpl->nonZeroControlCols[nonZeroIndexControl] = j;
+                                nonZeroIndexControl++;
+                            }
+                        }
+                    } else {
+                        std::ostringstream errorMsg;
+                        errorMsg << "The group " << group.second->group_ptr->name() <<" has dense control jacobian and no dynamical system has been provided yet. Cannot determine the sparsity since the control dimension is unknown.";
+                        reportError("OptimalControlProblem", "constraintJacobianWRTControlSparsity", errorMsg.str().c_str());
+                        return false;
+                    }
+                }
+
+                offset += group.second->controlJacobianBuffer.rows();
+            }
+            m_pimpl->nonZeroControlRows.resize(nonZeroIndexControl); //remove leftovers
+            m_pimpl->nonZeroControlCols.resize(nonZeroIndexControl); //remove leftovers
+
+            nonZeroElementRows = m_pimpl->nonZeroControlRows;
+            nonZeroElementColumns = m_pimpl->nonZeroControlCols;
+
+            return true;
+        }
+
     }
 }

From 4a32aade378d4e050526a47536e2e9b61a00e415 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 17 Oct 2018 19:16:45 +0200
Subject: [PATCH 074/194] Avoiding constraining the initial state.

---
 .../src/MultipleShootingSolver.cpp            | 59 ++++++++++++-------
 1 file changed, 38 insertions(+), 21 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 01fcc791606..8334c5b3180 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -1073,12 +1073,7 @@ namespace iDynTree {
                             }
                         }
 
-                        //Saving the jacobian structure due to the constraints
-                        if (ocHasStateSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateNZRows, m_ocStateNZCols);
-                        } else {
-                            addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
-                        }
+                        //Saving the jacobian structure due to the constraints (state should not be constrained here)
                         if (ocHasControlSparsisty) {
                             addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
                         } else {
@@ -1735,7 +1730,14 @@ namespace iDynTree {
                         constraintIndex += nx;
                     }
 
-                    if (!(m_ocproblem->constraintsEvaluation(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsBuffer))){
+                    bool okConstraint = true;
+                    if (mesh->origin == first) {
+                        okConstraint = m_ocproblem->constraintsEvaluation(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_constraintsBuffer);
+
+                    } else {
+                        okConstraint = m_ocproblem->constraintsEvaluation(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsBuffer);
+                    }
+                    if (!okConstraint){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating the constraints at time " << mesh->time << ".";
                         reportError("MultipleShootingTranscription", "evaluateConstraints", errorMsg.str().c_str());
@@ -1811,23 +1813,38 @@ namespace iDynTree {
                         constraintIndex += nx;
                     }
 
-                    if (!(m_ocproblem->constraintsJacobianWRTState(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsStateJacBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating the constraints state jacobian at time " << mesh->time << ".";
-                        reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
-                        return false;
-                    }
 
-                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nc, nx) = toEigen(m_constraintsStateJacBuffer);                    
+                    if (mesh->origin == first) {
 
-                    if (!(m_ocproblem->constraintsJacobianWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsControlJacBuffer))){
-                        std::ostringstream errorMsg;
-                        errorMsg << "Error while evaluating the constraints control jacobian at time " << mesh->time << ".";
-                        reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
-                        return false;
-                    }
+                        if (!(m_ocproblem->constraintsJacobianWRTControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_constraintsControlJacBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating the constraints control jacobian at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->controlIndex), nc, nu) = toEigen(m_constraintsControlJacBuffer);
+
+                    } else {
+
+                        if (!(m_ocproblem->constraintsJacobianWRTState(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsStateJacBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating the constraints state jacobian at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nc, nx) = toEigen(m_constraintsStateJacBuffer);
 
-                    jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->controlIndex), nc, nu) = toEigen(m_constraintsControlJacBuffer);
+                        if (!(m_ocproblem->constraintsJacobianWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_constraintsControlJacBuffer))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating the constraints control jacobian at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateConstraintsJacobian", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->controlIndex), nc, nu) = toEigen(m_constraintsControlJacBuffer);
+                    }
                     constraintIndex += nc;
                 }
                 assert(static_cast<size_t>(constraintIndex) == m_numberOfConstraints);

From faad7a4163a48dc5500f79c02f12f3c49b3ddb43 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 18 Oct 2018 11:57:43 +0200
Subject: [PATCH 075/194] Fixed bug when computing dense structure of
 constraints in OptimalControlProblem.

---
 src/optimalcontrol/src/OptimalControlProblem.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 5aa2cc27770..1210f55150d 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -1134,8 +1134,8 @@ namespace iDynTree {
 
                     if (m_pimpl->dynamicalSystem) {
 
-                        size_t rows = group.second->controlJacobianBuffer.rows();
-                        size_t cols = group.second->controlJacobianBuffer.cols();
+                        size_t rows = group.second->stateJacobianBuffer.rows();
+                        size_t cols = group.second->stateJacobianBuffer.cols();
                         size_t nonZeros = rows * cols;
 
                         if (m_pimpl->nonZeroStateRows.size() < (nonZeroIndexState + nonZeros)) {

From 534f72fc9852ef51389b5e1316ad1a3cbe0f19d6 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 18 Oct 2018 16:44:26 +0200
Subject: [PATCH 076/194] Moved sparsity vectors in a standalone class.

---
 src/core/include/iDynTree/Core/Utils.h        |  3 -
 src/core/src/Utils.cpp                        | 11 ---
 src/optimalcontrol/CMakeLists.txt             |  6 +-
 .../include/iDynTree/Constraint.h             | 11 ++-
 .../include/iDynTree/ConstraintsGroup.h       | 11 ++-
 .../include/iDynTree/DynamicalSystem.h        | 11 ++-
 .../include/iDynTree/Integrator.h             | 11 +--
 .../iDynTree/Integrators/ForwardEuler.h       |  8 +-
 .../Integrators/ImplicitTrapezoidal.h         |  8 +-
 .../include/iDynTree/OptimalControlProblem.h  |  5 +-
 .../include/iDynTree/SparsityStructure.h      | 49 ++++++++++++
 src/optimalcontrol/src/Constraint.cpp         |  4 +-
 src/optimalcontrol/src/ConstraintsGroup.cpp   | 37 ++++-----
 src/optimalcontrol/src/DynamicalSystem.cpp    |  4 +-
 src/optimalcontrol/src/ForwardEuler.cpp       | 10 +--
 .../src/ImplicitTrapezoidal.cpp               | 10 +--
 src/optimalcontrol/src/Integrator.cpp         |  4 +-
 .../src/MultipleShootingSolver.cpp            | 45 +++++------
 .../src/OptimalControlProblem.cpp             | 79 ++++++++-----------
 src/optimalcontrol/src/SparsityStructure.cpp  | 67 ++++++++++++++++
 20 files changed, 238 insertions(+), 156 deletions(-)
 create mode 100644 src/optimalcontrol/include/iDynTree/SparsityStructure.h
 create mode 100644 src/optimalcontrol/src/SparsityStructure.cpp

diff --git a/src/core/include/iDynTree/Core/Utils.h b/src/core/include/iDynTree/Core/Utils.h
index b45ab208dfe..a5405d0d030 100644
--- a/src/core/include/iDynTree/Core/Utils.h
+++ b/src/core/include/iDynTree/Core/Utils.h
@@ -12,7 +12,6 @@
 #define IDYNTREE_UTILS_H
 
 #include <cstddef>
-#include <vector>
 
 /**
  * \brief Macro to suppress unused variable warnings
@@ -138,8 +137,6 @@ namespace iDynTree
      */
     bool checkDoublesAreEqual(const double & val1, const double & val2, double tol = DEFAULT_TOL);
 
-    void addNonZeroIfNotPresent(size_t newRow, size_t newCol, std::vector<size_t>& nonZerosRows, std::vector<size_t>& nonZerosCols);
-
 }
 
 #endif
diff --git a/src/core/src/Utils.cpp b/src/core/src/Utils.cpp
index 9a001e3edfa..cc3dcb3fd62 100644
--- a/src/core/src/Utils.cpp
+++ b/src/core/src/Utils.cpp
@@ -101,16 +101,5 @@ namespace iDynTree
         return (std::fabs(val1-val2) < tol);
     }
 
-    void addNonZeroIfNotPresent(size_t newRow, size_t newCol, std::vector<size_t> &nonZerosRows, std::vector<size_t> &nonZerosCols)
-    {
-        for (size_t i = 0; i < nonZerosRows.size(); ++i) {
-            if ((newRow == nonZerosRows[i]) && (newCol == nonZerosCols[i])) {
-                return;
-            }
-        }
-        nonZerosRows.push_back(newRow);
-        nonZerosCols.push_back(newCol);
-    }
-
 
 }
diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index 041504e49df..c2bc2732921 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -36,7 +36,8 @@ set(PUBLIC_HEADERS include/iDynTree/OptimalControl.h
                    include/iDynTree/ConstraintsGroup.h
                    include/iDynTree/OptimizationProblem.h
                    include/iDynTree/Optimizer.h
-                   include/iDynTree/TimeVaryingObject.h)
+                   include/iDynTree/TimeVaryingObject.h
+                   include/iDynTree/SparsityStructure.h)
 set(INTEGRATORS_PUBLIC_HEADERS include/iDynTree/Integrators/FixedStepIntegrator.h
                                include/iDynTree/Integrators/RK4.h
                                include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -71,7 +72,8 @@ set(SOURCES src/DynamicalSystem.cpp
             src/OptimizationProblem.cpp
             src/Optimizer.cpp
             src/ForwardEuler.cpp
-            src/TimeVaryingObject.cpp)
+            src/TimeVaryingObject.cpp
+            src/SparsityStructure.cpp)
 
 list(APPEND OPTIMIZERS_HEADERS include/iDynTree/Optimizers/IpoptInterface.h)
 if (IDYNTREE_USES_IPOPT)
diff --git a/src/optimalcontrol/include/iDynTree/Constraint.h b/src/optimalcontrol/include/iDynTree/Constraint.h
index fc3c7073db0..4161ccaffb1 100644
--- a/src/optimalcontrol/include/iDynTree/Constraint.h
+++ b/src/optimalcontrol/include/iDynTree/Constraint.h
@@ -20,6 +20,7 @@
 #include <cstddef>
 #include <string>
 #include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/SparsityStructure.h>
 
 namespace iDynTree {
 
@@ -166,21 +167,19 @@ namespace iDynTree {
              * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
              *
              * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, stateDimension) respectively.
-             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
-             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @param stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
              * @return true if the sparsity is available. False otherwise.
              */
-            virtual bool constraintJacobianWRTStateSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+            virtual bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
 
             /**
              * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
              *
              * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
-             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
-             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @param controlSparsity Sparsity structure of the partial derivative of the jacobian wrt control variables.
              * @return true if the sparsity is available. False otherwise.
              */
-            virtual bool constraintJacobianWRTControlSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+            virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
 
 
         private:
diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index 92a2766c061..ccad846413a 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -20,6 +20,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <iDynTree/SparsityStructure.h>
 
 namespace iDynTree {
 
@@ -193,19 +194,17 @@ namespace iDynTree {
 
             /**
              * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
-             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
-             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @param stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
              * @return true if the sparsity is available. False otherwise.
              */
-            bool constraintJacobianWRTStateSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) const;
+            bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) const;
 
             /**
              * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
-             * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
-             * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+             * @param controlSparsity Sparsity structure of the partial derivative of the jacobian wrt control variables.
              * @return true if the sparsity is available. False otherwise.
              */
-            bool constraintJacobianWRTControlSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns) const;
+            bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) const;
 
             /**
              * @brief Flag returning true if the group is an "AnyTime" group.
diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index bd647775258..5754e41fa1e 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -18,6 +18,7 @@
 #define IDYNTREE_OPTIMALCONTROL_DYNAMICALSYSTEM_H
 
 #include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/SparsityStructure.h>
 
 namespace iDynTree {
 
@@ -172,19 +173,17 @@ namespace optimalcontrol {
 
         /**
          * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
-         * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
-         * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+         * @param stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
          * @return true if the sparsity is available. False otherwise.
          */
-        virtual bool dynamicsStateFirstDerivativeSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+        virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
 
         /**
          * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
-         * @param nonZeroElementRows The row indeces of non-zero elements. The corresponding columns are in the nonZeroElementColumns vector.
-         * @param nonZeroElementColumns The column indeces of non-zero elements. The corresponding rows are in the nonZeroElementRows vector.
+         * @param controlSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
          * @return true if the sparsity is available. False otherwise.
          */
-        virtual bool dynamicsControlFirstDerivativeSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+        virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
 
     private:
         size_t m_stateSize;
diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index b46c6e80c28..7b48080d1cb 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -19,6 +19,7 @@
 
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/SparsityStructure.h>
 #include <vector>
 #include <string>
 #include <memory>
@@ -52,12 +53,6 @@ namespace optimalcontrol {
                 double time;
             };
 
-            typedef struct {
-                std::vector<size_t> nonZeroElementRows;
-                std::vector<size_t> nonZeroElementColumns;
-            } CollocationSparsityVectors;
-
-
             class IntegratorInfoData {
             protected:
                 friend class Integrator;
@@ -239,9 +234,9 @@ namespace optimalcontrol {
                                                                    std::vector<MatrixDynSize>& stateJacobianValues,
                                                                    std::vector<MatrixDynSize>& controlJacobianValues);
 
-                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity);
+                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure>& stateJacobianSparsity);
 
-                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity);
+                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure>& controlJacobianSparsity);
 
 
                 const IntegratorInfo& info() const;
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
index 9fb64bdd37d..f04449c6ac9 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
@@ -37,8 +37,8 @@ namespace iDynTree {
                 MatrixDynSize m_stateJacBuffer, m_controlJacBuffer, m_identity, m_zeroBuffer;
                 bool m_hasStateSparsity = false;
                 bool m_hasControlSparsity = false;
-                std::vector<CollocationSparsityVectors> m_stateJacobianSparsity;
-                std::vector<CollocationSparsityVectors> m_controlJacobianSparsity;
+                std::vector<SparsityStructure> m_stateJacobianSparsity;
+                std::vector<SparsityStructure> m_controlJacobianSparsity;
 
                 bool allocateBuffers() override;
 
@@ -61,9 +61,9 @@ namespace iDynTree {
                                                            std::vector<MatrixDynSize> &stateJacobianValues,
                                                            std::vector<MatrixDynSize> &controlJacobianValues) override;
 
-                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity) override;
+                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure>& stateJacobianSparsity) override;
 
-                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity) override;
+                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure>& controlJacobianSparsity) override;
 
             };
         }
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
index 376e2ea4d7c..4567fd1dba4 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -37,8 +37,8 @@ namespace iDynTree {
                 MatrixDynSize m_identity, m_stateJacBuffer, m_controlJacBuffer;
                 bool m_hasStateSparsity = false;
                 bool m_hasControlSparsity = false;
-                std::vector<CollocationSparsityVectors> m_stateJacobianSparsity;
-                std::vector<CollocationSparsityVectors> m_controlJacobianSparsity;
+                std::vector<SparsityStructure> m_stateJacobianSparsity;
+                std::vector<SparsityStructure> m_controlJacobianSparsity;
 
                 virtual bool allocateBuffers() override;
 
@@ -61,9 +61,9 @@ namespace iDynTree {
                                                                    std::vector<MatrixDynSize> &stateJacobianValues,
                                                                    std::vector<MatrixDynSize> &controlJacobianValues) override;
 
-                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors>& stateJacobianSparsity) override;
+                virtual bool getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure>& stateJacobianSparsity) override;
 
-                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors>& controlJacobianSparsity) override;
+                virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure>& controlJacobianSparsity) override;
 
             };
 
diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index 08bc8b3a97c..d70ccd24f02 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -22,6 +22,7 @@
 #include <string>
 
 #include <iDynTree/Core/Utils.h>
+#include <iDynTree/SparsityStructure.h>
 
 namespace iDynTree {
 
@@ -225,9 +226,9 @@ namespace iDynTree {
                                                const VectorDynSize& control,
                                                MatrixDynSize& jacobian);
 
-            bool constraintJacobianWRTStateSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+            bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
 
-            bool constraintJacobianWRTControlSparsity(std::vector<size_t>& nonZeroElementRows, std::vector<size_t>& nonZeroElementColumns);
+            bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
 
         private:
             class OptimalControlProblemPimpl;
diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
new file mode 100644
index 00000000000..b55c498ee45
--- /dev/null
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#ifndef IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H
+#define IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H
+
+#include <vector>
+#include <cstddef>
+
+namespace iDynTree {
+    namespace optimalcontrol {
+        class SparsityStructure;
+    }
+}
+
+class iDynTree::optimalcontrol::SparsityStructure {
+public:
+    std::vector<size_t> nonZeroElementRows;
+    std::vector<size_t> nonZeroElementColumns;
+
+    SparsityStructure();
+
+    ~SparsityStructure();
+
+    bool merge(const SparsityStructure& other);
+
+    void addNonZeroIfNotPresent(size_t newRow, size_t newCol);
+
+    void resize(size_t newSize);
+
+    size_t size() const;
+
+    bool isValid() const;
+};
+
+#endif // IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H
diff --git a/src/optimalcontrol/src/Constraint.cpp b/src/optimalcontrol/src/Constraint.cpp
index a6acb10bf17..9bf756fa382 100644
--- a/src/optimalcontrol/src/Constraint.cpp
+++ b/src/optimalcontrol/src/Constraint.cpp
@@ -133,12 +133,12 @@ namespace iDynTree {
             return 0;
         }
 
-        bool Constraint::constraintJacobianWRTStateSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        bool Constraint::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
         {
             return false;
         }
 
-        bool Constraint::constraintJacobianWRTControlSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        bool Constraint::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
         {
             return false;
         }
diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index 8d7332ee1a4..684a76a82f3 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -48,8 +48,8 @@ namespace optimalcontrol {
         public:
             GroupOfConstraintsMap group;
             std::vector<TimedConstraint_ptr> orderedIntervals;
-            std::vector<size_t> nonZeroStateRows, nonZeroStateCols;
-            std::vector<size_t> nonZeroControlRows, nonZeroControlCols;
+            SparsityStructure groupStateSparsity;
+            SparsityStructure groupControlSparsity;
             std::string name;
             unsigned int maxConstraintSize;
             std::vector<TimeRange> timeRanges;
@@ -123,36 +123,31 @@ namespace optimalcontrol {
                 orderedIntervals.push_back(result.first->second); //register the time range in order to have the constraints ordered by init time. result.first->second is the TimedConstraint_ptr of the newly inserted TimedConstraint.
                 std::sort(orderedIntervals.begin(), orderedIntervals.end(), [](const TimedConstraint_ptr&a, const TimedConstraint_ptr&b) { return a->timeRange < b->timeRange;}); //reorder the vector
 
-                std::vector<size_t> newConstraintNNZRowsState, newConstraintNNZRowsControl;
-                std::vector<size_t> newConstraintNNZColsState, newConstraintNNZColsControl;
+                SparsityStructure newConstraintStateSparsity, newConstraintControlSparsity;
 
                 if (stateSparsityProvided
-                        && constraint->constraintJacobianWRTStateSparsity(newConstraintNNZRowsState, newConstraintNNZColsState)) {
+                        && constraint->constraintJacobianWRTStateSparsity(newConstraintStateSparsity)) {
 
-                    if (newConstraintNNZRowsState.size() != newConstraintNNZColsState.size()) {
+                    if (!newConstraintStateSparsity.isValid()) {
                         reportError("ConstraintsGroup", "addConstraint", "The state sparsity is provided but the dimension of the two vectors do not match.");
                         return false;
                     }
 
-                    for (size_t i = 0; i < newConstraintNNZRowsState.size(); ++i) {
-                        addNonZeroIfNotPresent(newConstraintNNZRowsState[i], newConstraintNNZColsState[i], nonZeroStateRows, nonZeroStateCols);
-                    }
+                    groupStateSparsity.merge(newConstraintStateSparsity);
 
                 } else {
                     stateSparsityProvided = false;
                 }
 
                 if (controlSparsityProvided
-                        && constraint->constraintJacobianWRTControlSparsity(newConstraintNNZRowsControl, newConstraintNNZColsControl)) {
+                        && constraint->constraintJacobianWRTControlSparsity(newConstraintControlSparsity)) {
 
-                    if (newConstraintNNZRowsControl.size() != newConstraintNNZColsControl.size()) {
+                    if (!newConstraintControlSparsity.isValid()) {
                         reportError("ConstraintsGroup", "addConstraint", "The control sparsity is provided but the dimension of the two vectors do not match.");
                         return false;
                     }
 
-                    for (size_t i = 0; i < newConstraintNNZRowsControl.size(); ++i) {
-                        addNonZeroIfNotPresent(newConstraintNNZRowsControl[i], newConstraintNNZColsControl[i], nonZeroControlRows, nonZeroControlCols);
-                    }
+                    groupControlSparsity.merge(newConstraintControlSparsity);
 
                 } else {
                     controlSparsityProvided = false;
@@ -285,7 +280,7 @@ namespace optimalcontrol {
             }
 
             size_t i=0;
-            for (auto constraint : m_pimpl->group) {
+            for (auto& constraint : m_pimpl->group) {
                 m_pimpl->timeRanges[i] = constraint.second->timeRange;
                 ++i;
             }
@@ -528,26 +523,24 @@ namespace optimalcontrol {
             return true;
         }
 
-        bool ConstraintsGroup::constraintJacobianWRTStateSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns) const
+        bool ConstraintsGroup::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity) const
         {
             if (!(m_pimpl->stateSparsityProvided)) {
                 return false;
             }
 
-            nonZeroElementRows = m_pimpl->nonZeroStateRows;
-            nonZeroElementColumns = m_pimpl->nonZeroStateCols;
+            stateSparsity = m_pimpl->groupStateSparsity;
 
             return true;
         }
 
-        bool ConstraintsGroup::constraintJacobianWRTControlSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns) const
+        bool ConstraintsGroup::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity) const
         {
             if (!(m_pimpl->controlSparsityProvided)) {
                 return false;
             }
 
-            nonZeroElementRows = m_pimpl->nonZeroControlRows;
-            nonZeroElementColumns = m_pimpl->nonZeroControlCols;
+            controlSparsity = m_pimpl->groupControlSparsity;
 
             return true;
         }
@@ -568,7 +561,7 @@ namespace optimalcontrol {
         const std::vector<std::string> ConstraintsGroup::listConstraints() const
         {
             std::vector<std::string> output;
-            for (auto constraint: m_pimpl->group) {
+            for (auto& constraint: m_pimpl->group) {
                 output.push_back(constraint.second->constraint->name()); //MEMORY ALLOCATION
             }
             return output;
diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp
index cd30256778b..972a2b06ee8 100644
--- a/src/optimalcontrol/src/DynamicalSystem.cpp
+++ b/src/optimalcontrol/src/DynamicalSystem.cpp
@@ -90,12 +90,12 @@ namespace iDynTree {
                                                              MatrixDynSize& dynamicsDerivative)
         { return false; }
 
-        bool DynamicalSystem::dynamicsStateFirstDerivativeSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        bool DynamicalSystem::dynamicsStateFirstDerivativeSparsity(SparsityStructure &stateSparsity)
         {
             return false;
         }
 
-        bool DynamicalSystem::dynamicsControlFirstDerivativeSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        bool DynamicalSystem::dynamicsControlFirstDerivativeSparsity(SparsityStructure &controlSparsity)
         {
             return false;
         }
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index 9f6fdc537d6..82626b9d0fd 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -48,12 +48,12 @@ namespace iDynTree {
                 m_stateJacobianSparsity.resize(2);
                 m_controlJacobianSparsity.resize(2);
 
-                if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns)) {
+                if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0])) {
                     m_stateJacobianSparsity[1].nonZeroElementRows.clear();
                     m_stateJacobianSparsity[1].nonZeroElementColumns.clear();
 
                     for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
-                        addNonZeroIfNotPresent(i, i, m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns);
+                        m_stateJacobianSparsity[0].addNonZeroIfNotPresent(i, i);
                         m_stateJacobianSparsity[1].nonZeroElementRows.push_back(i);
                         m_stateJacobianSparsity[1].nonZeroElementColumns.push_back(i);
                     }
@@ -61,7 +61,7 @@ namespace iDynTree {
                     m_hasStateSparsity = true;
                 }
 
-                if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0].nonZeroElementRows, m_controlJacobianSparsity[0].nonZeroElementColumns)) {
+                if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0])) {
                     m_controlJacobianSparsity[1].nonZeroElementRows.clear();
                     m_controlJacobianSparsity[1].nonZeroElementColumns.clear();
 
@@ -242,7 +242,7 @@ namespace iDynTree {
                 return true;
             }
 
-            bool ForwardEuler::getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors> &stateJacobianSparsity)
+            bool ForwardEuler::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &stateJacobianSparsity)
             {
                 if (!m_hasStateSparsity) {
                     return false;
@@ -252,7 +252,7 @@ namespace iDynTree {
                 return true;
             }
 
-            bool ForwardEuler::getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors> &controlJacobianSparsity)
+            bool ForwardEuler::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &controlJacobianSparsity)
             {
                 if (!m_hasControlSparsity) {
                     return false;
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index 2f58f05b442..6db2ea50ddd 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -48,17 +48,17 @@ namespace iDynTree {
                 m_stateJacobianSparsity.resize(2);
                 m_controlJacobianSparsity.resize(2);
 
-                if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns)) {
+                if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0])) {
 
                     for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
-                        addNonZeroIfNotPresent(i, i, m_stateJacobianSparsity[0].nonZeroElementRows, m_stateJacobianSparsity[0].nonZeroElementColumns);
+                        m_stateJacobianSparsity[0].addNonZeroIfNotPresent(i, i);
                     }
                     m_stateJacobianSparsity[1] = m_stateJacobianSparsity[0];
 
                     m_hasStateSparsity = true;
                 }
 
-                if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0].nonZeroElementRows, m_controlJacobianSparsity[0].nonZeroElementColumns)) {
+                if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0])) {
 
                     m_controlJacobianSparsity[1] = m_controlJacobianSparsity[0];
 
@@ -250,7 +250,7 @@ namespace iDynTree {
 
             }
 
-            bool ImplicitTrapezoidal::getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors> &stateJacobianSparsity)
+            bool ImplicitTrapezoidal::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &stateJacobianSparsity)
             {
                 if (!m_hasStateSparsity) {
                     return false;
@@ -260,7 +260,7 @@ namespace iDynTree {
                 return true;
             }
 
-            bool ImplicitTrapezoidal::getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors> &controlJacobianSparsity)
+            bool ImplicitTrapezoidal::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &controlJacobianSparsity)
             {
                 if (!m_hasControlSparsity) {
                     return false;
diff --git a/src/optimalcontrol/src/Integrator.cpp b/src/optimalcontrol/src/Integrator.cpp
index bb85870a8b7..3ee3c50e0f2 100644
--- a/src/optimalcontrol/src/Integrator.cpp
+++ b/src/optimalcontrol/src/Integrator.cpp
@@ -129,12 +129,12 @@ namespace iDynTree {
                 return false;
             }
 
-            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<CollocationSparsityVectors> &stateJacobianSparsity)
+            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &stateJacobianSparsity)
             {
                 return false;
             }
 
-            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<CollocationSparsityVectors> &controlJacobianSparsity)
+            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &controlJacobianSparsity)
             {
                 return false;
             }
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 8334c5b3180..26f16923f2f 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -16,6 +16,7 @@
 
 #include <iDynTree/OCSolvers/MultipleShootingSolver.h>
 
+#include <iDynTree/SparsityStructure.h>
 #include <iDynTree/OptimalControlProblem.h>
 #include <iDynTree/DynamicalSystem.h>
 #include <iDynTree/Integrator.h>
@@ -97,9 +98,9 @@ namespace iDynTree {
             double m_minStepSize, m_maxStepSize, m_controlPeriod;
             size_t m_nx, m_nu, m_numberOfVariables, m_constraintsPerInstant, m_numberOfConstraints;
             std::vector<size_t> m_jacobianNZRows, m_jacobianNZCols, m_hessianNZRows, m_hessianNZCols;
-            std::vector<size_t> m_ocStateNZRows, m_ocStateNZCols, m_ocControlNZRows, m_ocControlNZCols;
-            std::vector<integrators::CollocationSparsityVectors> m_collocationStateNZ, m_collocationControlNZ;
-            integrators::CollocationSparsityVectors m_mergedCollocationControlNZ;
+            SparsityStructure m_ocStateSparsity, m_ocControlSparsity;
+            std::vector<SparsityStructure> m_collocationStateNZ, m_collocationControlNZ;
+            SparsityStructure m_mergedCollocationControlNZ;
             size_t m_jacobianNonZeros, m_hessianNonZeros;
             double m_plusInfinity, m_minusInfinity;
             VectorDynSize m_constraintsLowerBound, m_constraintsUpperBound;
@@ -187,10 +188,10 @@ namespace iDynTree {
                 }
             }
 
-            void addJacobianBlock(size_t initRow, size_t initCol, const std::vector<size_t> &nonZeroRows, const std::vector<size_t> &nonZeroCols){
-                for (size_t i = 0; i < nonZeroRows.size(); ++i) {
-                    addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + nonZeroRows[i]);
-                    addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + nonZeroCols[i]);
+            void addJacobianBlock(size_t initRow, size_t initCol, const SparsityStructure& sparsity){
+                for (size_t i = 0; i < sparsity.size(); ++i) {
+                    addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + sparsity.nonZeroElementRows[i]);
+                    addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + sparsity.nonZeroElementColumns[i]);
                     m_jacobianNonZeros++;
                 }
             }
@@ -213,7 +214,7 @@ namespace iDynTree {
                 }
             }
 
-            void mergeSparsityVectors(const std::vector<integrators::CollocationSparsityVectors>& original, integrators::CollocationSparsityVectors& merged) {
+            void mergeSparsityVectors(const std::vector<SparsityStructure>& original, SparsityStructure& merged) {
                 const std::vector<size_t>& firstRows = original[0].nonZeroElementRows;
                 const std::vector<size_t>& firstCols = original[0].nonZeroElementColumns;
                 const std::vector<size_t>& secondRows = original[1].nonZeroElementRows;
@@ -1013,12 +1014,12 @@ namespace iDynTree {
                 Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(m_constraintsLowerBound);
                 Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(m_constraintsUpperBound);
 
-                bool ocHasStateSparsisty = m_ocproblem->constraintJacobianWRTStateSparsity(m_ocStateNZRows, m_ocStateNZCols);
+                bool ocHasStateSparsisty = m_ocproblem->constraintJacobianWRTStateSparsity(m_ocStateSparsity);
                 if (!ocHasStateSparsisty) {
                     reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve state sparsity of optimal control problem constraints. Assuming dense matrix.");
                 }
 
-                bool ocHasControlSparsisty = m_ocproblem->constraintJacobianWRTControlSparsity(m_ocControlNZRows, m_ocControlNZCols);
+                bool ocHasControlSparsisty = m_ocproblem->constraintJacobianWRTControlSparsity(m_ocControlSparsity);
                 if (!ocHasControlSparsisty) {
                     reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve control sparsity of optimal control problem constraints. Assuming dense matrix.");
                 }
@@ -1075,7 +1076,7 @@ namespace iDynTree {
 
                         //Saving the jacobian structure due to the constraints (state should not be constrained here)
                         if (ocHasControlSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         }
@@ -1108,16 +1109,16 @@ namespace iDynTree {
 
                         //Saving the jacobian structure due to the dynamical constraints
                         if (systemHasControlSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_collocationControlNZ[1].nonZeroElementRows, m_collocationControlNZ[1].nonZeroElementColumns);
-                            addJacobianBlock(constraintIndex, mesh->previousControlIndex, m_collocationControlNZ[0].nonZeroElementRows, m_collocationControlNZ[0].nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_collocationControlNZ[1]);
+                            addJacobianBlock(constraintIndex, mesh->previousControlIndex, m_collocationControlNZ[0]);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
                             addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
                         }
 
                         if (systemHasStateSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1].nonZeroElementRows, m_collocationStateNZ[1].nonZeroElementColumns);
-                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0].nonZeroElementRows, m_collocationStateNZ[0].nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1]);
+                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0]);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
                             addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
@@ -1150,12 +1151,12 @@ namespace iDynTree {
 
                         //Saving the jacobian structure due to the constraints
                         if (ocHasStateSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateNZRows, m_ocStateNZCols);
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         }
                         if (ocHasControlSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         }
@@ -1191,14 +1192,14 @@ namespace iDynTree {
 
                         //Saving the jacobian structure due to the dynamical constraints
                         if (systemHasControlSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_mergedCollocationControlNZ.nonZeroElementRows, m_mergedCollocationControlNZ.nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_mergedCollocationControlNZ);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
                         }
 
                         if (systemHasStateSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1].nonZeroElementRows, m_collocationStateNZ[1].nonZeroElementColumns);
-                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0].nonZeroElementRows, m_collocationStateNZ[0].nonZeroElementColumns);
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1]);
+                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0]);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
                             addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
@@ -1229,12 +1230,12 @@ namespace iDynTree {
                         }
 
                         if (ocHasStateSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateNZRows, m_ocStateNZCols);
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         }
                         if (ocHasControlSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlNZRows, m_ocControlNZCols);
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         }
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 1210f55150d..0541c9eb332 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -45,8 +45,8 @@ namespace iDynTree {
             std::shared_ptr<ConstraintsGroup> group_ptr;
             VectorDynSize constraintsBuffer;
             MatrixDynSize stateJacobianBuffer, controlJacobianBuffer;
-            std::vector<size_t> nonZeroStateRows, nonZeroStateCols;
-            std::vector<size_t> nonZeroControlRows, nonZeroControlCols;
+            SparsityStructure stateSparsity;
+            SparsityStructure controlSparsity;
             bool hasStateSparsity = false;
             bool hasControlSparsity = false;
         } BufferedGroup;
@@ -76,8 +76,8 @@ namespace iDynTree {
             MatrixDynSize costStateHessianBuffer, costControlHessianBuffer, costMixedHessianBuffer;
             bool stateLowerBounded, stateUpperBounded, controlLowerBounded, controlUpperBounded;
             VectorDynSize stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
-            std::vector<size_t> nonZeroStateRows, nonZeroStateCols;
-            std::vector<size_t> nonZeroControlRows, nonZeroControlCols;
+            SparsityStructure stateSparsity;
+            SparsityStructure controlSparsity;
             std::vector<std::string> mayerCostnames;
             std::vector<TimeRange> constraintsTimeRanges, costTimeRanges;
             std::vector<size_t> linearConstraintIndeces;
@@ -213,8 +213,8 @@ namespace iDynTree {
                 newGroup->controlJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize()));
             }
 
-            newGroup->hasStateSparsity = groupOfConstraints->constraintJacobianWRTStateSparsity(newGroup->nonZeroStateRows, newGroup->nonZeroStateCols); //needed to allocate memory in advance
-            newGroup->hasControlSparsity = groupOfConstraints->constraintJacobianWRTControlSparsity(newGroup->nonZeroControlRows, newGroup->nonZeroControlCols); //needed to allocate memory in advance
+            newGroup->hasStateSparsity = groupOfConstraints->constraintJacobianWRTStateSparsity(newGroup->stateSparsity); //needed to allocate memory in advance
+            newGroup->hasControlSparsity = groupOfConstraints->constraintJacobianWRTControlSparsity(newGroup->controlSparsity); //needed to allocate memory in advance
 
             std::pair< ConstraintsGroupsMap::iterator, bool> groupResult;
             groupResult = m_pimpl->constraintsGroups.insert(std::pair< std::string, BufferedGroup_ptr>(groupOfConstraints->name(), newGroup));
@@ -226,11 +226,9 @@ namespace iDynTree {
                 return false;
             }
 
-            m_pimpl->nonZeroStateRows.resize(m_pimpl->nonZeroStateRows.size() + newGroup->nonZeroStateRows.size()); //needed to allocate memory in advance
-            m_pimpl->nonZeroStateCols.resize(m_pimpl->nonZeroStateCols.size() + newGroup->nonZeroStateCols.size()); //needed to allocate memory in advance
+            m_pimpl->stateSparsity.resize(m_pimpl->stateSparsity.size() + newGroup->stateSparsity.size()); //needed to allocate memory in advance
 
-            m_pimpl->nonZeroControlRows.resize(m_pimpl->nonZeroControlRows.size() + newGroup->nonZeroControlRows.size()); //needed to allocate memory in advance
-            m_pimpl->nonZeroControlCols.resize(m_pimpl->nonZeroControlCols.size() + newGroup->nonZeroControlCols.size()); //needed to allocate memory in advance
+            m_pimpl->controlSparsity.resize(m_pimpl->controlSparsity.size() + newGroup->controlSparsity.size()); //needed to allocate memory in advance
 
             return true;
         }
@@ -1109,24 +1107,23 @@ namespace iDynTree {
             return true;
         }
 
-        bool OptimalControlProblem::constraintJacobianWRTStateSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        bool OptimalControlProblem::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
         {
             size_t nonZeroIndexState = 0;
             size_t offset = 0;
             for (auto& group : m_pimpl->constraintsGroups){
 
-                group.second->hasStateSparsity = group.second->group_ptr->constraintJacobianWRTStateSparsity(group.second->nonZeroStateRows, group.second->nonZeroStateCols);
+                group.second->hasStateSparsity = group.second->group_ptr->constraintJacobianWRTStateSparsity(group.second->stateSparsity);
 
                 if (group.second->hasStateSparsity) {
 
-                    if (m_pimpl->nonZeroStateRows.size() < (nonZeroIndexState + group.second->nonZeroStateRows.size())) {
-                        m_pimpl->nonZeroStateRows.resize(nonZeroIndexState + group.second->nonZeroStateRows.size());
-                         m_pimpl->nonZeroStateCols.resize(nonZeroIndexState + group.second->nonZeroStateRows.size());
+                    if (m_pimpl->stateSparsity.size() < (nonZeroIndexState + group.second->stateSparsity.size())) {
+                        m_pimpl->stateSparsity.resize(nonZeroIndexState + group.second->stateSparsity.size());
                     }
 
-                    for (size_t i = 0; i < group.second->nonZeroStateRows.size(); ++i) {
-                        m_pimpl->nonZeroStateRows[nonZeroIndexState] = group.second->nonZeroStateRows[i] + offset;
-                        m_pimpl->nonZeroStateCols[nonZeroIndexState] = group.second->nonZeroStateCols[i];
+                    for (size_t i = 0; i < group.second->stateSparsity.size(); ++i) {
+                        m_pimpl->stateSparsity.nonZeroElementRows[nonZeroIndexState]    = group.second->stateSparsity.nonZeroElementRows[i] + offset;
+                        m_pimpl->stateSparsity.nonZeroElementColumns[nonZeroIndexState] = group.second->stateSparsity.nonZeroElementColumns[i];
                         nonZeroIndexState++;
                     }
 
@@ -1138,15 +1135,14 @@ namespace iDynTree {
                         size_t cols = group.second->stateJacobianBuffer.cols();
                         size_t nonZeros = rows * cols;
 
-                        if (m_pimpl->nonZeroStateRows.size() < (nonZeroIndexState + nonZeros)) {
-                            m_pimpl->nonZeroStateRows.resize(nonZeroIndexState + nonZeros);
-                             m_pimpl->nonZeroStateCols.resize(nonZeroIndexState + nonZeros);
+                        if (m_pimpl->stateSparsity.size() < (nonZeroIndexState + nonZeros)) {
+                            m_pimpl->stateSparsity.resize(nonZeroIndexState + nonZeros);
                         }
 
                         for (size_t i = 0; i < rows; ++i) {
                             for (size_t j = 0; j < cols; ++j) {
-                                m_pimpl->nonZeroStateRows[nonZeroIndexState] = i + offset;
-                                m_pimpl->nonZeroStateCols[nonZeroIndexState] = j;
+                                m_pimpl->stateSparsity.nonZeroElementRows[nonZeroIndexState]    = i + offset;
+                                m_pimpl->stateSparsity.nonZeroElementColumns[nonZeroIndexState] = j;
                                 nonZeroIndexState++;
                             }
                         }
@@ -1160,34 +1156,31 @@ namespace iDynTree {
 
                 offset += group.second->stateJacobianBuffer.rows();
             }
-            m_pimpl->nonZeroStateRows.resize(nonZeroIndexState); //remove leftovers
-            m_pimpl->nonZeroStateCols.resize(nonZeroIndexState); //remove leftovers
+            m_pimpl->stateSparsity.resize(nonZeroIndexState); //remove leftovers
 
 
-            nonZeroElementRows = m_pimpl->nonZeroStateRows;
-            nonZeroElementColumns = m_pimpl->nonZeroStateCols;
+            stateSparsity = m_pimpl->stateSparsity;
 
             return true;
         }
 
-        bool OptimalControlProblem::constraintJacobianWRTControlSparsity(std::vector<size_t> &nonZeroElementRows, std::vector<size_t> &nonZeroElementColumns)
+        bool OptimalControlProblem::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
         {
             size_t nonZeroIndexControl = 0;
             size_t offset = 0;
             for (auto& group : m_pimpl->constraintsGroups){
 
-                group.second->hasControlSparsity = group.second->group_ptr->constraintJacobianWRTControlSparsity(group.second->nonZeroControlRows, group.second->nonZeroControlCols);
+                group.second->hasControlSparsity = group.second->group_ptr->constraintJacobianWRTControlSparsity(group.second->controlSparsity);
 
                 if (group.second->hasControlSparsity) {
 
-                    if (m_pimpl->nonZeroControlRows.size() < (nonZeroIndexControl + group.second->nonZeroControlRows.size())) {
-                        m_pimpl->nonZeroControlRows.resize(nonZeroIndexControl + group.second->nonZeroControlRows.size());
-                         m_pimpl->nonZeroControlCols.resize(nonZeroIndexControl + group.second->nonZeroControlRows.size());
+                    if (m_pimpl->controlSparsity.size() < (nonZeroIndexControl + group.second->controlSparsity.size())) {
+                        m_pimpl->controlSparsity.resize(nonZeroIndexControl + group.second->controlSparsity.size());
                     }
 
-                    for (size_t i = 0; i < group.second->nonZeroControlRows.size(); ++i) {
-                        m_pimpl->nonZeroControlRows[nonZeroIndexControl] = group.second->nonZeroControlRows[i] + offset;
-                        m_pimpl->nonZeroControlCols[nonZeroIndexControl] = group.second->nonZeroControlCols[i];
+                    for (size_t i = 0; i < group.second->controlSparsity.size(); ++i) {
+                        m_pimpl->controlSparsity.nonZeroElementRows[nonZeroIndexControl]    = group.second->controlSparsity.nonZeroElementRows[i] + offset;
+                        m_pimpl->controlSparsity.nonZeroElementColumns[nonZeroIndexControl] = group.second->controlSparsity.nonZeroElementColumns[i];
                         nonZeroIndexControl++;
                     }
 
@@ -1199,15 +1192,14 @@ namespace iDynTree {
                         size_t cols = group.second->controlJacobianBuffer.cols();
                         size_t nonZeros = rows * cols;
 
-                        if (m_pimpl->nonZeroControlRows.size() < (nonZeroIndexControl + nonZeros)) {
-                            m_pimpl->nonZeroControlRows.resize(nonZeroIndexControl + nonZeros);
-                             m_pimpl->nonZeroControlCols.resize(nonZeroIndexControl + nonZeros);
+                        if (m_pimpl->controlSparsity.size() < (nonZeroIndexControl + nonZeros)) {
+                            m_pimpl->controlSparsity.resize(nonZeroIndexControl + nonZeros);
                         }
 
                         for (size_t i = 0; i < rows; ++i) {
                             for (size_t j = 0; j < cols; ++j) {
-                                m_pimpl->nonZeroControlRows[nonZeroIndexControl] = i + offset;
-                                m_pimpl->nonZeroControlCols[nonZeroIndexControl] = j;
+                                m_pimpl->controlSparsity.nonZeroElementRows[nonZeroIndexControl]    = i + offset;
+                                m_pimpl->controlSparsity.nonZeroElementColumns[nonZeroIndexControl] = j;
                                 nonZeroIndexControl++;
                             }
                         }
@@ -1221,11 +1213,10 @@ namespace iDynTree {
 
                 offset += group.second->controlJacobianBuffer.rows();
             }
-            m_pimpl->nonZeroControlRows.resize(nonZeroIndexControl); //remove leftovers
-            m_pimpl->nonZeroControlCols.resize(nonZeroIndexControl); //remove leftovers
+            m_pimpl->controlSparsity.resize(nonZeroIndexControl); //remove leftovers
 
-            nonZeroElementRows = m_pimpl->nonZeroControlRows;
-            nonZeroElementColumns = m_pimpl->nonZeroControlCols;
+
+            controlSparsity = m_pimpl->controlSparsity;
 
             return true;
         }
diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp
new file mode 100644
index 00000000000..42579c65ace
--- /dev/null
+++ b/src/optimalcontrol/src/SparsityStructure.cpp
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2014,2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ *
+ * Originally developed for Prioritized Optimal Control (2014)
+ * Refactored in 2018.
+ * Design inspired by
+ * - ACADO toolbox (http://acado.github.io)
+ * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
+ */
+
+#include <iDynTree/SparsityStructure.h>
+#include <iDynTree/Core/Utils.h>
+#include <cassert>
+
+iDynTree::optimalcontrol::SparsityStructure::SparsityStructure()
+{ }
+
+iDynTree::optimalcontrol::SparsityStructure::~SparsityStructure()
+{ }
+
+bool iDynTree::optimalcontrol::SparsityStructure::merge(const iDynTree::optimalcontrol::SparsityStructure &other)
+{
+    if (!other.isValid()) {
+        reportError("SparsityStructure", "merge", "The other SparsityStructure vectors have different size.");
+        return false;
+    }
+
+    for (size_t i = 0; i < other.nonZeroElementRows.size(); ++i) {
+        addNonZeroIfNotPresent(other.nonZeroElementRows[i], other.nonZeroElementColumns[i]);
+    }
+
+    return true;
+}
+
+void iDynTree::optimalcontrol::SparsityStructure::addNonZeroIfNotPresent(size_t newRow, size_t newCol)
+{
+    for (size_t i = 0; i < nonZeroElementRows.size(); ++i) {
+        if ((newRow == nonZeroElementRows[i]) && (newCol == nonZeroElementColumns[i])) {
+            return;
+        }
+    }
+    nonZeroElementRows.push_back(newRow);
+    nonZeroElementColumns.push_back(newCol);
+}
+
+void iDynTree::optimalcontrol::SparsityStructure::resize(size_t newSize)
+{
+    nonZeroElementRows.resize(newSize);
+    nonZeroElementColumns.resize(newSize);
+}
+
+size_t iDynTree::optimalcontrol::SparsityStructure::size() const
+{
+    assert(isValid());
+    return nonZeroElementRows.size();
+}
+
+bool iDynTree::optimalcontrol::SparsityStructure::isValid() const
+{
+    return (nonZeroElementColumns.size() == nonZeroElementRows.size());
+}

From 4481121143270e37fdccffc6c225c803e496b253 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 19 Oct 2018 15:19:10 +0200
Subject: [PATCH 077/194] Implemented finer sparsity for linear objects.

---
 .../include/iDynTree/LinearConstraint.h       | 11 ++++
 .../include/iDynTree/LinearSystem.h           |  9 ++++
 src/optimalcontrol/src/LinearConstraint.cpp   | 41 +++++++++++++++
 src/optimalcontrol/src/LinearSystem.cpp       | 50 +++++++++++++++++++
 4 files changed, 111 insertions(+)

diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
index 7f7ed9070b5..b3f66ee97a8 100644
--- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h
+++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
@@ -20,6 +20,7 @@
 #include <iDynTree/Constraint.h>
 #include <string>
 #include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/SparsityStructure.h>
 
 namespace iDynTree {
     namespace optimalcontrol {
@@ -42,6 +43,10 @@ namespace iDynTree {
 
             LinearConstraint(size_t size, const std::string name);
 
+            LinearConstraint(size_t size, const std::string name,
+                             const SparsityStructure& stateSparsity,
+                             const SparsityStructure& controlSparsity);
+
             virtual ~LinearConstraint() override;
 
             bool setStateConstraintMatrix(const MatrixDynSize& constraintMatrix);
@@ -63,11 +68,17 @@ namespace iDynTree {
                                                       const VectorDynSize& control,
                                                       MatrixDynSize& jacobian) final;
 
+            virtual bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) final;
+
+            virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
+
         private:
             bool m_constrainsState, m_constrainsControl;
             iDynTree::MatrixDynSize m_stateConstraintMatrix;
             iDynTree::MatrixDynSize m_controlConstraintMatrix;
             iDynTree::VectorDynSize m_stateConstraintsBuffer, m_controlConstraintsBuffer;
+            bool m_hasStateSparsity, m_hasControlSparsity;
+            SparsityStructure m_stateSparsity, m_controlSparsity;
         };
 
     }
diff --git a/src/optimalcontrol/include/iDynTree/LinearSystem.h b/src/optimalcontrol/include/iDynTree/LinearSystem.h
index a58963718d8..6ef672bb8a3 100644
--- a/src/optimalcontrol/include/iDynTree/LinearSystem.h
+++ b/src/optimalcontrol/include/iDynTree/LinearSystem.h
@@ -19,6 +19,7 @@
 
 #include <iDynTree/DynamicalSystem.h>
 #include <iDynTree/TimeVaryingObject.h>
+#include <iDynTree/SparsityStructure.h>
 #include <memory>
 
 namespace iDynTree {
@@ -39,6 +40,10 @@ namespace iDynTree {
 
             LinearSystem(size_t stateSize, size_t controlSize);
 
+            LinearSystem(size_t stateSize, size_t controlSize,
+                         const iDynTree::optimalcontrol::SparsityStructure& stateSparsity,
+                         const iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
+
             LinearSystem(const LinearSystem& other) = delete;
 
             ~LinearSystem() override;
@@ -65,6 +70,10 @@ namespace iDynTree {
                                                         double time,
                                                         MatrixDynSize& dynamicsDerivative) final;
 
+            virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) final;
+
+            virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
+
 
         private:
             class LinearSystemPimpl;
diff --git a/src/optimalcontrol/src/LinearConstraint.cpp b/src/optimalcontrol/src/LinearConstraint.cpp
index 25dee96a939..5877a32f1f0 100644
--- a/src/optimalcontrol/src/LinearConstraint.cpp
+++ b/src/optimalcontrol/src/LinearConstraint.cpp
@@ -29,11 +29,34 @@ namespace iDynTree {
         , m_constrainsControl(false)
         , m_stateConstraintsBuffer(static_cast<unsigned int>(size))
         , m_controlConstraintsBuffer(static_cast<unsigned int>(size))
+        , m_hasStateSparsity(false)
+        , m_hasControlSparsity(false)
         {
             m_stateConstraintsBuffer.zero();
             m_controlConstraintsBuffer.zero();
         }
 
+        LinearConstraint::LinearConstraint(size_t size, const std::string name, const SparsityStructure &stateSparsity, const SparsityStructure &controlSparsity)
+            : Constraint(size, name)
+            , m_constrainsState(false)
+            , m_constrainsControl(false)
+            , m_stateConstraintsBuffer(static_cast<unsigned int>(size))
+            , m_controlConstraintsBuffer(static_cast<unsigned int>(size))
+            , m_hasStateSparsity(false)
+            , m_hasControlSparsity(false)
+        {
+            m_stateConstraintsBuffer.zero();
+            m_controlConstraintsBuffer.zero();
+            if (stateSparsity.isValid()) {
+                m_hasStateSparsity = true;
+                m_stateSparsity = stateSparsity;
+            }
+            if (controlSparsity.isValid()) {
+                m_hasControlSparsity = true;
+                m_controlSparsity = controlSparsity;
+            }
+        }
+
         LinearConstraint::~LinearConstraint() {}
 
         bool LinearConstraint::setStateConstraintMatrix(const MatrixDynSize &constraintMatrix)
@@ -139,5 +162,23 @@ namespace iDynTree {
             return true;
         }
 
+        bool LinearConstraint::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            if (!m_hasStateSparsity) {
+                return false;
+            }
+            stateSparsity = m_stateSparsity;
+            return true;
+        }
+
+        bool LinearConstraint::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            if (!m_hasControlSparsity) {
+                return false;
+            }
+            controlSparsity = m_controlSparsity;
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp
index e8f5237c886..ea613dd3dd2 100644
--- a/src/optimalcontrol/src/LinearSystem.cpp
+++ b/src/optimalcontrol/src/LinearSystem.cpp
@@ -32,6 +32,9 @@ namespace iDynTree {
         public:
             std::shared_ptr<TimeVaryingMatrix> stateMatrix;
             std::shared_ptr<TimeVaryingMatrix> controlMatrix;
+
+            SparsityStructure stateSparsity, controlSparsity;
+            bool hasStateSparsity, hasControlSparsity;
         };
 
 
@@ -41,6 +44,8 @@ namespace iDynTree {
         , m_pimpl(new LinearSystemPimpl())
         {
             assert(m_pimpl);
+            m_pimpl->hasStateSparsity = false;
+            m_pimpl->hasControlSparsity = false;
             std::shared_ptr<TimeInvariantMatrix> tempState = std::make_shared<TimeInvariantMatrix>();
             tempState->get().resize(static_cast<unsigned int>(stateSize), static_cast<unsigned int>(stateSize));
             tempState->get().zero();
@@ -52,6 +57,33 @@ namespace iDynTree {
             m_pimpl->controlMatrix = tempControl;
         }
 
+        LinearSystem::LinearSystem(size_t stateSize, size_t controlSize, const SparsityStructure &stateSparsity, const SparsityStructure &controlSparsity)
+            : DynamicalSystem(stateSize, controlSize)
+            , m_pimpl(new LinearSystemPimpl())
+        {
+            assert(m_pimpl);
+            std::shared_ptr<TimeInvariantMatrix> tempState = std::make_shared<TimeInvariantMatrix>();
+            tempState->get().resize(static_cast<unsigned int>(stateSize), static_cast<unsigned int>(stateSize));
+            tempState->get().zero();
+            m_pimpl->stateMatrix = tempState;
+
+            std::shared_ptr<TimeInvariantMatrix> tempControl = std::make_shared<TimeInvariantMatrix>();
+            tempControl->get().resize(static_cast<unsigned int>(stateSize), static_cast<unsigned int>(controlSize));
+            tempControl->get().zero();
+            m_pimpl->controlMatrix = tempControl;
+
+            m_pimpl->hasStateSparsity = false;
+            m_pimpl->hasControlSparsity = false;
+            if (stateSparsity.isValid()) {
+                m_pimpl->hasStateSparsity = true;
+                m_pimpl->stateSparsity = stateSparsity;
+            }
+            if (controlSparsity.isValid()) {
+                m_pimpl->hasControlSparsity = true;
+                m_pimpl->controlSparsity = controlSparsity;
+            }
+        }
+
         LinearSystem::~LinearSystem()
         {
             if(m_pimpl){
@@ -193,5 +225,23 @@ namespace iDynTree {
             return true;
         }
 
+        bool LinearSystem::dynamicsStateFirstDerivativeSparsity(SparsityStructure &stateSparsity)
+        {
+            if (!(m_pimpl->hasStateSparsity)) {
+                return false;
+            }
+            stateSparsity = m_pimpl->stateSparsity;
+            return true;
+        }
+
+        bool LinearSystem::dynamicsControlFirstDerivativeSparsity(SparsityStructure &controlSparsity)
+        {
+            if (!(m_pimpl->hasControlSparsity)) {
+                return false;
+            }
+            controlSparsity = m_pimpl->controlSparsity;
+            return true;
+        }
+
     }
 }

From 7bb924edbf77b1c51e7f1430aec76c243d74c8a2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 19 Oct 2018 15:45:04 +0200
Subject: [PATCH 078/194] Hidden implementation of LinearConstraint.

---
 .../include/iDynTree/LinearConstraint.h       |  16 +-
 src/optimalcontrol/src/LinearConstraint.cpp   | 204 ++++++++++++------
 2 files changed, 149 insertions(+), 71 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
index b3f66ee97a8..d780bc73bd5 100644
--- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h
+++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
@@ -18,9 +18,11 @@
 #define IDYNTREE_OPTIMALCONTROL_LINEARCONSTRAINT_H
 
 #include <iDynTree/Constraint.h>
-#include <string>
 #include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/TimeVaryingObject.h>
 #include <iDynTree/SparsityStructure.h>
+#include <string>
+#include <memory>
 
 namespace iDynTree {
     namespace optimalcontrol {
@@ -53,6 +55,10 @@ namespace iDynTree {
 
             bool setControlConstraintMatrix(const MatrixDynSize& constraintMatrix);
 
+            bool setStateConstraintMatrix(std::shared_ptr<TimeVaryingMatrix> constraintMatrix);
+
+            bool setControlConstraintMatrix(std::shared_ptr<TimeVaryingMatrix> constraintMatrix);
+
             virtual bool evaluateConstraint(double time,
                                             const VectorDynSize& state,
                                             const VectorDynSize& control,
@@ -73,12 +79,8 @@ namespace iDynTree {
             virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
 
         private:
-            bool m_constrainsState, m_constrainsControl;
-            iDynTree::MatrixDynSize m_stateConstraintMatrix;
-            iDynTree::MatrixDynSize m_controlConstraintMatrix;
-            iDynTree::VectorDynSize m_stateConstraintsBuffer, m_controlConstraintsBuffer;
-            bool m_hasStateSparsity, m_hasControlSparsity;
-            SparsityStructure m_stateSparsity, m_controlSparsity;
+            class LinearConstraintImplementation;
+            LinearConstraintImplementation* m_pimpl;
         };
 
     }
diff --git a/src/optimalcontrol/src/LinearConstraint.cpp b/src/optimalcontrol/src/LinearConstraint.cpp
index 5877a32f1f0..948bc36110b 100644
--- a/src/optimalcontrol/src/LinearConstraint.cpp
+++ b/src/optimalcontrol/src/LinearConstraint.cpp
@@ -23,51 +23,64 @@
 namespace iDynTree {
     namespace optimalcontrol {
 
+        class LinearConstraint::LinearConstraintImplementation {
+        public:
+            bool constrainsState, constrainsControl;
+            std::shared_ptr<TimeVaryingMatrix> stateConstraintMatrix;
+            std::shared_ptr<TimeVaryingMatrix> controlConstraintMatrix;
+            iDynTree::VectorDynSize stateConstraintsBuffer, controlConstraintsBuffer;
+            bool hasStateSparsity, hasControlSparsity;
+            SparsityStructure stateSparsity, controlSparsity;
+
+            LinearConstraintImplementation(size_t size)
+                : constrainsState(false)
+                , constrainsControl(false)
+                , stateConstraintsBuffer(static_cast<unsigned int>(size))
+                , controlConstraintsBuffer(static_cast<unsigned int>(size))
+                , hasStateSparsity(false)
+                , hasControlSparsity(false)
+            {
+                stateConstraintsBuffer.zero();
+                controlConstraintsBuffer.zero();
+            }
+        };
+
         LinearConstraint::LinearConstraint(size_t size, const std::string name)
         : Constraint(size, name)
-        , m_constrainsState(false)
-        , m_constrainsControl(false)
-        , m_stateConstraintsBuffer(static_cast<unsigned int>(size))
-        , m_controlConstraintsBuffer(static_cast<unsigned int>(size))
-        , m_hasStateSparsity(false)
-        , m_hasControlSparsity(false)
-        {
-            m_stateConstraintsBuffer.zero();
-            m_controlConstraintsBuffer.zero();
-        }
+        , m_pimpl(new LinearConstraintImplementation(size))
+        { }
 
         LinearConstraint::LinearConstraint(size_t size, const std::string name, const SparsityStructure &stateSparsity, const SparsityStructure &controlSparsity)
             : Constraint(size, name)
-            , m_constrainsState(false)
-            , m_constrainsControl(false)
-            , m_stateConstraintsBuffer(static_cast<unsigned int>(size))
-            , m_controlConstraintsBuffer(static_cast<unsigned int>(size))
-            , m_hasStateSparsity(false)
-            , m_hasControlSparsity(false)
+            , m_pimpl(new LinearConstraintImplementation(size))
         {
-            m_stateConstraintsBuffer.zero();
-            m_controlConstraintsBuffer.zero();
             if (stateSparsity.isValid()) {
-                m_hasStateSparsity = true;
-                m_stateSparsity = stateSparsity;
+                m_pimpl->hasStateSparsity = true;
+                m_pimpl->stateSparsity = stateSparsity;
             }
             if (controlSparsity.isValid()) {
-                m_hasControlSparsity = true;
-                m_controlSparsity = controlSparsity;
+                m_pimpl->hasControlSparsity = true;
+                m_pimpl->controlSparsity = controlSparsity;
             }
         }
 
-        LinearConstraint::~LinearConstraint() {}
+        LinearConstraint::~LinearConstraint()
+        {
+            if (m_pimpl) {
+                delete m_pimpl;
+                m_pimpl = nullptr;
+            }
+        }
 
         bool LinearConstraint::setStateConstraintMatrix(const MatrixDynSize &constraintMatrix)
         {
             if (constraintMatrix.rows() != constraintSize()) {
-                reportError("LinearConstraint", "setStateConstraintMatrix", "The number of rows of the constraintMatrix is different from the specified constraint size.");
+                reportError(name().c_str(), "setStateConstraintMatrix", "The number of rows of the constraintMatrix is different from the specified constraint size.");
                 return false;
             }
 
-            m_stateConstraintMatrix = constraintMatrix;
-            m_constrainsState = true;
+            m_pimpl->stateConstraintMatrix = std::make_shared<TimeInvariantMatrix>(constraintMatrix);
+            m_pimpl->constrainsState = true;
 
             return true;
         }
@@ -75,13 +88,33 @@ namespace iDynTree {
         bool LinearConstraint::setControlConstraintMatrix(const MatrixDynSize &constraintMatrix)
         {
             if (constraintMatrix.rows() != constraintSize()) {
-                reportError("LinearConstraint", "setControlConstraintMatrix", "The number of rows of the constraintMatrix is different from the specified constraint size.");
+                reportError(name().c_str(), "setControlConstraintMatrix", "The number of rows of the constraintMatrix is different from the specified constraint size.");
                 return false;
             }
 
-            m_controlConstraintMatrix = constraintMatrix;
-            m_constrainsControl = true;
+            m_pimpl->controlConstraintMatrix = std::make_shared<TimeInvariantMatrix>(constraintMatrix);
+            m_pimpl->constrainsControl = true;
+
+            return true;
+        }
+
+        bool LinearConstraint::setStateConstraintMatrix(std::shared_ptr<TimeVaryingMatrix> constraintMatrix)
+        {
+            if (!constraintMatrix) {
+                reportError(name().c_str(), "setStateConstraintMatrix", "Empty constraintMatrix pointer.");
+                return false;
+            }
+            m_pimpl->stateConstraintMatrix = constraintMatrix;
+            return true;
+        }
 
+        bool LinearConstraint::setControlConstraintMatrix(std::shared_ptr<TimeVaryingMatrix> constraintMatrix)
+        {
+            if (!constraintMatrix) {
+                reportError(name().c_str(), "setControlConstraintMatrix", "Empty constraintMatrix pointer.");
+                return false;
+            }
+            m_pimpl->controlConstraintMatrix = constraintMatrix;
             return true;
         }
 
@@ -91,92 +124,135 @@ namespace iDynTree {
                                                   const VectorDynSize& control,
                                                   VectorDynSize& constraint)
         {
-            if (m_constrainsState) {
-                if (m_stateConstraintMatrix.cols() != state.size()) {
-                    reportError("LinearConstraint", "evaluateConstraint", "The dimension of the specified constraint matrix and the state do not match.");
+            if (m_pimpl->constrainsState) {
+                bool isValid = false;
+                const MatrixDynSize& stateConstraintMatrix = m_pimpl->stateConstraintMatrix->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state constraint matrix at time: " << time << ".";
+                    reportError(name().c_str(), "evaluateConstraint", errorMsg.str().c_str());
                     return false;
                 }
-                iDynTree::iDynTreeEigenMatrixMap stateConstraint = iDynTree::toEigen(m_stateConstraintMatrix);
 
-                iDynTree::toEigen(m_stateConstraintsBuffer) = stateConstraint * iDynTree::toEigen(state);
+                if ((stateConstraintMatrix.cols() != state.size()) || (stateConstraintMatrix.rows() != constraintSize())) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state constraint matrix at time: " << time << " has dimensions not matching with the specified state space dimension or constraint dimension.";
+                    reportError(name().c_str(), "evaluateConstraint", errorMsg.str().c_str());
+                }
+                iDynTree::iDynTreeEigenConstMatrixMap stateConstraint = iDynTree::toEigen(stateConstraintMatrix);
+
+                iDynTree::toEigen(m_pimpl->stateConstraintsBuffer) = stateConstraint * iDynTree::toEigen(state);
             }
 
 
-            if (m_constrainsControl) {
-                if (m_controlConstraintMatrix.cols() != control.size()) {
-                    reportError("LinearConstraint", "evaluateConstraint", "The dimension of the specified constraint matrix and the control do not match.");
+            if (m_pimpl->constrainsControl) {
+                bool isValid = false;
+                const MatrixDynSize& controlConstraintMatrix = m_pimpl->controlConstraintMatrix->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control constraint matrix at time: " << time << ".";
+                    reportError(name().c_str(), "evaluateConstraint", errorMsg.str().c_str());
                     return false;
                 }
 
-                iDynTree::iDynTreeEigenMatrixMap controlConstraint = iDynTree::toEigen(m_controlConstraintMatrix);
-                iDynTree::toEigen(m_controlConstraintsBuffer) = controlConstraint * iDynTree::toEigen(control);
+                if ((controlConstraintMatrix.cols() != control.size()) || (controlConstraintMatrix.rows() != constraintSize())) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control constraint matrix at time: " << time << " has dimensions not matching with the specified control space dimension or constraint dimension.";
+                    reportError(name().c_str(), "evaluateConstraint", errorMsg.str().c_str());
+                }
+
+                iDynTree::iDynTreeEigenConstMatrixMap controlConstraint = iDynTree::toEigen(controlConstraintMatrix);
+                iDynTree::toEigen(m_pimpl->controlConstraintsBuffer) = controlConstraint * iDynTree::toEigen(control);
             }
 
             constraint.resize(static_cast<unsigned int>(constraintSize()));
-            toEigen(constraint) = iDynTree::toEigen(m_stateConstraintsBuffer) + iDynTree::toEigen(m_controlConstraintsBuffer); //the buffers are zero if not constrained
+            toEigen(constraint) = iDynTree::toEigen(m_pimpl->stateConstraintsBuffer) + iDynTree::toEigen(m_pimpl->controlConstraintsBuffer); //the buffers are zero if not constrained
 
             return true;
 
         }
 
-        bool LinearConstraint::constraintJacobianWRTState(double /*time*/,
+        bool LinearConstraint::constraintJacobianWRTState(double time,
                                                 const VectorDynSize& state,
                                                 const VectorDynSize& /*control*/,
                                                 MatrixDynSize& jacobian)
         {
-            if (m_constrainsState) {
-                if (m_stateConstraintMatrix.cols() != state.size()) {
-                    reportError("LinearConstraint", "constraintJacobianWRTState", "The dimension of the specified constraint matrix and the state do not match.");
+            jacobian.resize(static_cast<unsigned int>(constraintSize()), state.size());
+
+            if (m_pimpl->constrainsState) {
+                bool isValid = false;
+                const MatrixDynSize& stateConstraintMatrix = m_pimpl->stateConstraintMatrix->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid state constraint matrix at time: " << time << ".";
+                    reportError(name().c_str(), "constraintJacobianWRTState", errorMsg.str().c_str());
                     return false;
                 }
-            } else {
-                if ((m_stateConstraintMatrix.rows() != static_cast<unsigned int>(constraintSize())) ||
-                        (m_stateConstraintMatrix.cols() != state.size())) {
-                    m_stateConstraintMatrix.resize(static_cast<unsigned int>(constraintSize()), state.size());
-                    m_stateConstraintMatrix.zero();
+
+                if ((stateConstraintMatrix.cols() != state.size()) || (stateConstraintMatrix.rows() != constraintSize())) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state constraint matrix at time: " << time << " has dimensions not matching with the specified state space dimension or constraint dimension.";
+                    reportError(name().c_str(), "constraintJacobianWRTState", errorMsg.str().c_str());
                 }
+
+                jacobian = stateConstraintMatrix;
+            } else {
+                jacobian.zero();
             }
-            jacobian = m_stateConstraintMatrix;
 
             return true;
         }
 
-        bool LinearConstraint::constraintJacobianWRTControl(double /*time*/,
+        bool LinearConstraint::constraintJacobianWRTControl(double time,
                                                   const VectorDynSize& /*state*/,
                                                   const VectorDynSize& control,
                                                   MatrixDynSize& jacobian)
         {
-            if (m_constrainsControl) {
-                if (m_controlConstraintMatrix.cols() != control.size()) {
-                    reportError("LinearConstraint", "constraintJacobianWRTControl", "The dimension of the specified constraint matrix and the control do not match.");
+            jacobian.resize(static_cast<unsigned int>(constraintSize()), control.size());
+
+            if (m_pimpl->constrainsControl) {
+                bool isValid = false;
+                const MatrixDynSize& controlConstraintMatrix = m_pimpl->controlConstraintMatrix->get(time, isValid);
+
+                if (!isValid) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Unable to retrieve a valid control constraint matrix at time: " << time << ".";
+                    reportError(name().c_str(), "constraintJacobianWRTControl", errorMsg.str().c_str());
                     return false;
                 }
-            } else {
-                if ((m_controlConstraintMatrix.rows() != static_cast<unsigned int>(constraintSize())) ||
-                        (m_controlConstraintMatrix.cols() != control.size())) {
-                    m_controlConstraintMatrix.resize(static_cast<unsigned int>(constraintSize()), control.size());
-                    m_controlConstraintMatrix.zero();
+
+                if ((controlConstraintMatrix.cols() != control.size()) || (controlConstraintMatrix.rows() != constraintSize())) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The control constraint matrix at time: " << time << " has dimensions not matching with the specified control space dimension or constraint dimension.";
+                    reportError(name().c_str(), "constraintJacobianWRTControl", errorMsg.str().c_str());
                 }
+
+                jacobian = controlConstraintMatrix;
+            } else {
+                jacobian.zero();
             }
-            jacobian = m_controlConstraintMatrix;
+
             return true;
         }
 
         bool LinearConstraint::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
         {
-            if (!m_hasStateSparsity) {
+            if (!m_pimpl->hasStateSparsity) {
                 return false;
             }
-            stateSparsity = m_stateSparsity;
+            stateSparsity = m_pimpl->stateSparsity;
             return true;
         }
 
         bool LinearConstraint::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
         {
-            if (!m_hasControlSparsity) {
+            if (!m_pimpl->hasControlSparsity) {
                 return false;
             }
-            controlSparsity = m_controlSparsity;
+            controlSparsity = m_pimpl->controlSparsity;
             return true;
         }
 

From 9ec06edccce343584efaefd8f5b0d405b25287f2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 19 Oct 2018 17:11:10 +0200
Subject: [PATCH 079/194] Added some utility methods for the sparsity
 structure.

---
 .../include/iDynTree/SparsityStructure.h      |  15 +++
 .../src/MultipleShootingSolver.cpp            |  10 +-
 src/optimalcontrol/src/SparsityStructure.cpp  | 107 +++++++++++++++++-
 3 files changed, 120 insertions(+), 12 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
index b55c498ee45..2c04ce3b6cb 100644
--- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -17,6 +17,7 @@
 #ifndef IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H
 #define IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H
 
+#include <iDynTree/Core/Utils.h>
 #include <vector>
 #include <cstddef>
 
@@ -37,10 +38,24 @@ class iDynTree::optimalcontrol::SparsityStructure {
 
     bool merge(const SparsityStructure& other);
 
+    void addDenseBlock(size_t startRow, size_t startColumn, size_t numberOfRows, size_t numberOfColumns);
+
+    bool addDenseBlock(long startRow, long startColumn, long numberOfRows, long numberOfColumns);
+
+    bool addDenseBlock(const iDynTree::IndexRange& rowsRange, const iDynTree::IndexRange& columnsRange);
+
+    void addIdentityBlock(size_t startRow, size_t startColumn, size_t dimension);
+
+    bool addIdentityBlock(long startRow, long startColumn, long dimension);
+
     void addNonZeroIfNotPresent(size_t newRow, size_t newCol);
 
+    bool isValuePresent(size_t row, size_t col) const;
+
     void resize(size_t newSize);
 
+    void clear();
+
     size_t size() const;
 
     bool isValid() const;
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 26f16923f2f..d1c4c21009e 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -227,14 +227,7 @@ namespace iDynTree {
                 }
 
                 for (size_t j = 0; j < secondRows.size(); ++j) {
-                    bool duplicate = false;
-                    size_t i = 0;
-                    while (!duplicate && (i < firstRows.size())) {
-                        if ((secondRows[j] == firstRows[i]) && (secondCols[j] == firstCols[i])) {
-                            duplicate = true;
-                        }
-                        ++i;
-                    }
+                    bool duplicate = original[0].isValuePresent(secondRows[j], secondCols[j]);
 
                     if (!duplicate) {
                         addNonZero(merged.nonZeroElementRows, mergedNonZeros, secondRows[j]);
@@ -242,6 +235,7 @@ namespace iDynTree {
                         mergedNonZeros++;
                     }
                 }
+                merged.resize(mergedNonZeros);
             }
 
             void allocateBuffers(){
diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp
index 42579c65ace..57972675165 100644
--- a/src/optimalcontrol/src/SparsityStructure.cpp
+++ b/src/optimalcontrol/src/SparsityStructure.cpp
@@ -38,15 +38,108 @@ bool iDynTree::optimalcontrol::SparsityStructure::merge(const iDynTree::optimalc
     return true;
 }
 
+void iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(size_t startRow, size_t startColumn, size_t numberOfRows, size_t numberOfColumns)
+{
+    for (size_t i = 0; i < numberOfRows; ++i) {
+        for (size_t j = 0; j < numberOfColumns; ++j) {
+            addNonZeroIfNotPresent(startRow + i, startColumn + j);
+        }
+    }
+}
+
+bool iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(long startRow, long startColumn, long numberOfRows, long numberOfColumns)
+{
+    if (startRow < 0) {
+        reportError("SparsityStructure", "addDenseBlock", "The startRow is negative.");
+        return false;
+    }
+
+    if (startColumn < 0) {
+        reportError("SparsityStructure", "addDenseBlock", "The startColumn is negative.");
+        return false;
+    }
+
+    if (numberOfRows < 0) {
+        reportError("SparsityStructure", "addDenseBlock", "The numberOfRows is negative.");
+        return false;
+    }
+
+    if (numberOfColumns < 0) {
+        reportError("SparsityStructure", "addDenseBlock", "The numberOfColumns is negative.");
+        return false;
+    }
+
+    addDenseBlock(static_cast<size_t>(startRow),
+                  static_cast<size_t>(startColumn),
+                  static_cast<size_t>(numberOfRows),
+                  static_cast<size_t>(numberOfColumns));
+    return true;
+}
+
+bool iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(const iDynTree::IndexRange &rowsRange, const iDynTree::IndexRange &columnsRange)
+{
+    if (!rowsRange.isValid()) {
+        reportError("SparsityStructure", "addDenseBlock", "The rowsRange is not valid.");
+        return false;
+    }
+
+    if (!columnsRange.isValid()) {
+        reportError("SparsityStructure", "addDenseBlock", "The columnsRange is not valid.");
+        return false;
+    }
+
+    addDenseBlock(rowsRange.offset, columnsRange.offset, rowsRange.size, columnsRange.size);
+
+    return true;
+}
+
+void iDynTree::optimalcontrol::SparsityStructure::addIdentityBlock(size_t startRow, size_t startColumn, size_t dimension)
+{
+    for (size_t i = 0; i < dimension; ++i) {
+        addNonZeroIfNotPresent(startRow + i, startColumn + i);
+    }
+}
+
+bool iDynTree::optimalcontrol::SparsityStructure::addIdentityBlock(long startRow, long startColumn, long dimension)
+{
+    if (startRow < 0) {
+        reportError("SparsityStructure", "addIdentityBlock", "The startRow is negative.");
+        return false;
+    }
+
+    if (startColumn < 0) {
+        reportError("SparsityStructure", "addIdentityBlock", "The startColumn is negative.");
+        return false;
+    }
+
+    if (dimension < 0) {
+        reportError("SparsityStructure", "addIdentityBlock", "The dimension is negative.");
+        return false;
+    }
+
+    addIdentityBlock(static_cast<size_t>(startRow),
+                     static_cast<size_t>(startColumn),
+                     static_cast<size_t>(dimension)
+                     );
+    return true;
+}
+
 void iDynTree::optimalcontrol::SparsityStructure::addNonZeroIfNotPresent(size_t newRow, size_t newCol)
+{
+    if (!isValuePresent(newRow, newCol)) {
+        nonZeroElementRows.push_back(newRow);
+        nonZeroElementColumns.push_back(newCol);
+    }
+}
+
+bool iDynTree::optimalcontrol::SparsityStructure::isValuePresent(size_t row, size_t col) const
 {
     for (size_t i = 0; i < nonZeroElementRows.size(); ++i) {
-        if ((newRow == nonZeroElementRows[i]) && (newCol == nonZeroElementColumns[i])) {
-            return;
+        if ((row == nonZeroElementRows[i]) && (col == nonZeroElementColumns[i])) {
+            return true;
         }
     }
-    nonZeroElementRows.push_back(newRow);
-    nonZeroElementColumns.push_back(newCol);
+    return false;
 }
 
 void iDynTree::optimalcontrol::SparsityStructure::resize(size_t newSize)
@@ -55,6 +148,12 @@ void iDynTree::optimalcontrol::SparsityStructure::resize(size_t newSize)
     nonZeroElementColumns.resize(newSize);
 }
 
+void iDynTree::optimalcontrol::SparsityStructure::clear()
+{
+    nonZeroElementRows.clear();
+    nonZeroElementColumns.clear();
+}
+
 size_t iDynTree::optimalcontrol::SparsityStructure::size() const
 {
     assert(isValid());

From 73de8040283d98b82c67c7db03ffc34033b76b25 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 19 Oct 2018 17:11:33 +0200
Subject: [PATCH 080/194] Bumped version to 0.11.2 to account for the
 optimalcontrol finer sparsity.

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1be9d7fce6a..5f7808e46ce 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,7 +25,7 @@ set(VARS_PREFIX "iDynTree")
 
 set(${VARS_PREFIX}_MAJOR_VERSION 0)
 set(${VARS_PREFIX}_MINOR_VERSION 11)
-set(${VARS_PREFIX}_PATCH_VERSION 1)
+set(${VARS_PREFIX}_PATCH_VERSION 2)
 set(${VARS_PREFIX}_VERSION ${${VARS_PREFIX}_MAJOR_VERSION}.${${VARS_PREFIX}_MINOR_VERSION}.${${VARS_PREFIX}_PATCH_VERSION})
 
 # Pick up our CMake scripts - they are all in the cmake subdirectory.

From 3272dabf271788e013ae7cf17476f41bad598725 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 22 Oct 2018 11:43:15 +0200
Subject: [PATCH 081/194] Use finer sparsity in tests.

---
 .../tests/MultipleShootingTest.cpp            | 29 ++++++++++
 src/optimalcontrol/tests/OCProblemTest.cpp    | 57 +++++++++++++++++++
 2 files changed, 86 insertions(+)

diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 8d08d602e8b..07bb6000710 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -77,6 +77,23 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
         dynamicsDerivative(1, 2) = 2.0;
         return true;
     }
+
+    virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override{
+        iDynTree::optimalcontrol::SparsityStructure sparsity;
+        sparsity.addIdentityBlock(0ul, 0, 2);
+        stateSparsity = sparsity;
+        return true;
+    }
+
+    virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
+        iDynTree::optimalcontrol::SparsityStructure sparsity;
+        sparsity.addNonZeroIfNotPresent(0, 0);
+        sparsity.addNonZeroIfNotPresent(0, 1);
+        sparsity.addNonZeroIfNotPresent(1, 0);
+        sparsity.addNonZeroIfNotPresent(1, 2);
+        controlSparsity = sparsity;
+        return true;
+    }
 };
 TestSystem::~TestSystem(){};
 
@@ -142,6 +159,18 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
     virtual size_t expectedControlSpaceSize() const override {
         return 3;
     }
+
+    virtual bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override {
+        stateSparsity.clear();
+        return true;
+    }
+
+    virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
+        iDynTree::optimalcontrol::SparsityStructure sparsity;
+        sparsity.addNonZeroIfNotPresent(0,0);
+        controlSparsity = sparsity;
+        return true;
+    }
 };
 TestConstraint::~TestConstraint(){}
 
diff --git a/src/optimalcontrol/tests/OCProblemTest.cpp b/src/optimalcontrol/tests/OCProblemTest.cpp
index 12263557df4..45c9591ea16 100644
--- a/src/optimalcontrol/tests/OCProblemTest.cpp
+++ b/src/optimalcontrol/tests/OCProblemTest.cpp
@@ -73,6 +73,23 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
         dynamicsDerivative(1, 2) = 2.0;
         return true;
     }
+
+    virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override{
+        iDynTree::optimalcontrol::SparsityStructure sparsity;
+        sparsity.addIdentityBlock(0ul, 0, 2);
+        stateSparsity = sparsity;
+        return true;
+    }
+
+    virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
+        iDynTree::optimalcontrol::SparsityStructure sparsity;
+        sparsity.addNonZeroIfNotPresent(0, 0);
+        sparsity.addNonZeroIfNotPresent(0, 1);
+        sparsity.addNonZeroIfNotPresent(1, 0);
+        sparsity.addNonZeroIfNotPresent(1, 2);
+        controlSparsity = sparsity;
+        return true;
+    }
 };
 TestSystem::~TestSystem(){};
 
@@ -138,6 +155,18 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
     virtual size_t expectedControlSpaceSize() const override {
         return 3;
     }
+
+    virtual bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override {
+        stateSparsity.clear();
+        return true;
+    }
+
+    virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
+        iDynTree::optimalcontrol::SparsityStructure sparsity;
+        sparsity.addNonZeroIfNotPresent(0,0);
+        controlSparsity = sparsity;
+        return true;
+    }
 };
 TestConstraint::~TestConstraint(){}
 
@@ -360,5 +389,33 @@ int main() {
     ASSERT_IS_TRUE(problem.isFeasiblePoint(3.0, testState, testControl));
     ASSERT_IS_FALSE(problem.isFeasiblePoint(4.0, testState, testControl));
 
+    iDynTree::optimalcontrol::SparsityStructure stateSparsity, controlSparsity;
+    ASSERT_IS_TRUE(problem.constraintJacobianWRTStateSparsity(stateSparsity));
+    ASSERT_IS_TRUE(problem.constraintJacobianWRTControlSparsity(controlSparsity));
+    iDynTree::MatrixDynSize stateSparsityCheck, controlSparsityCheck, stateZeroCheck, controlZeroCheck;
+    stateSparsityCheck = obtainedStatejac;
+    stateZeroCheck = stateSparsityCheck;
+    stateZeroCheck.zero();
+    controlSparsityCheck = obtainedControlJac;
+    controlZeroCheck = controlSparsityCheck;
+    controlZeroCheck.zero();
+
+    for(size_t i = 0; i < stateSparsity.size(); ++i) {
+        unsigned int row = static_cast<unsigned int>(stateSparsity.nonZeroElementRows[i]);
+        unsigned int col = static_cast<unsigned int>(stateSparsity.nonZeroElementColumns[i]);
+        stateSparsityCheck(row, col) = 0.0;
+    }
+
+    ASSERT_EQUAL_MATRIX(stateSparsityCheck, stateZeroCheck);
+
+    for(size_t i = 0; i < controlSparsity.size(); ++i) {
+        unsigned int row = static_cast<unsigned int>(controlSparsity.nonZeroElementRows[i]);
+        unsigned int col = static_cast<unsigned int>(controlSparsity.nonZeroElementColumns[i]);
+        controlSparsityCheck(row, col) = 0.0;
+    }
+
+    ASSERT_EQUAL_MATRIX(controlSparsityCheck, controlZeroCheck);
+
+
     return EXIT_SUCCESS;
 }

From 36577871e2520003765d7c8556c7bf0531c191f6 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 25 Oct 2018 17:53:49 +0200
Subject: [PATCH 082/194] Allow for time varying bounds.

---
 .../include/iDynTree/OptimalControlProblem.h  |  26 +++-
 .../src/MultipleShootingSolver.cpp            | 104 ++++++++++------
 .../src/OptimalControlProblem.cpp             | 114 ++++++++++++++++--
 3 files changed, 189 insertions(+), 55 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index d70ccd24f02..085d97bae9e 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -23,6 +23,8 @@
 
 #include <iDynTree/Core/Utils.h>
 #include <iDynTree/SparsityStructure.h>
+#include <iDynTree/TimeVaryingObject.h>
+
 
 namespace iDynTree {
 
@@ -160,25 +162,39 @@ namespace iDynTree {
 
             bool setStateLowerBound(const VectorDynSize& minState);
 
+            bool setStateLowerBound(std::shared_ptr<TimeVaryingVector> minState);
+
             bool setStateUpperBound(const VectorDynSize& maxState);
 
+            bool setStateUpperBound(std::shared_ptr<TimeVaryingVector> maxState);
+
             bool setControlLowerBound(const VectorDynSize& minControl);
 
+            bool setControlLowerBound(std::shared_ptr<TimeVaryingVector> minControl);
+
             bool setControlUpperBound(const VectorDynSize& maxControl);
 
+            bool setControlUpperBound(std::shared_ptr<TimeVaryingVector> maxControl);
+
             bool setStateBoxConstraints(const VectorDynSize& minState,
-                                        const VectorDynSize& maxState);
+                                        const VectorDynSize& maxState);            
+
+            bool setStateBoxConstraints(std::shared_ptr<TimeVaryingVector> minState,
+                                        std::shared_ptr<TimeVaryingVector> maxState);
 
             bool setControlBoxConstraints(const VectorDynSize& minControl,
                                           const VectorDynSize& maxControl);
 
-            bool getStateLowerBound(VectorDynSize& minState) const;  //return false if the corresponding bound is not set
+            bool setControlBoxConstraints(std::shared_ptr<TimeVaryingVector> minControl,
+                                          std::shared_ptr<TimeVaryingVector> maxControl);
+
+            bool getStateLowerBound(double time, VectorDynSize& minState);  //return false if the corresponding bound is not set
 
-            bool getStateUpperBound(VectorDynSize& maxState) const;  //return false if the corresponding bound is not set
+            bool getStateUpperBound(double time, VectorDynSize& maxState);  //return false if the corresponding bound is not set
 
-            bool getControlLowerBound(VectorDynSize& minControl) const;  //return false if the corresponding bound is not set
+            bool getControlLowerBound(double time, VectorDynSize& minControl);  //return false if the corresponding bound is not set
 
-            bool getControlUpperBound(VectorDynSize& maxControl) const;  //return false if the corresponding bound is not set
+            bool getControlUpperBound(double time, VectorDynSize& maxControl);  //return false if the corresponding bound is not set
 
             bool costsEvaluation(double time, const VectorDynSize& state, const VectorDynSize& control, double& costValue);
 
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index d1c4c21009e..061debc21f8 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -1298,24 +1298,9 @@ namespace iDynTree {
 
             virtual bool getVariablesUpperBound(VectorDynSize& variablesUpperBound) override {
 
-                bool stateBounded = true, controlBounded = true;
-
                 Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
                 Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
 
-                if (!(m_ocproblem->getStateUpperBound(m_stateBuffer))) {
-                    stateBounded = false;
-                    stateBufferMap.setConstant(m_plusInfinity);
-                }
-
-                if (!(m_ocproblem->getControlUpperBound(m_controlBuffer))) {
-                    controlBounded = false;
-                    controlBufferMap.setConstant(m_plusInfinity);
-                }
-
-                if (!controlBounded && !stateBounded) {
-                    return false;
-                }
 
                 if (variablesUpperBound.size() != m_numberOfVariables) {
                     variablesUpperBound.resize(numberOfVariables());
@@ -1326,15 +1311,43 @@ namespace iDynTree {
                 Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                bool isBounded;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
                         upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_plusInfinity); //avoiding setting bounds on the initial state
-                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+
+                        isBounded = m_ocproblem->getControlUpperBound(mesh->time, m_controlBuffer);
+                        if (isBounded) {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+                        } else {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu).setConstant(m_plusInfinity);
+                        }
+
                     } else if (mesh->type == MeshPointType::Control) {
-                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
-                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+
+                        isBounded = m_ocproblem->getControlUpperBound(mesh->time, m_controlBuffer);
+                        if (isBounded) {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+                        } else {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu).setConstant(m_plusInfinity);
+                        }
+
+                        isBounded = m_ocproblem->getStateUpperBound(mesh->time, m_stateBuffer);
+                        if (isBounded) {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+                        } else {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_plusInfinity);
+                        }
+
                     } else if (mesh->type == MeshPointType::State) {
-                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+
+                        isBounded = m_ocproblem->getStateUpperBound(mesh->time, m_stateBuffer);
+                        if (isBounded) {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+                        } else {
+                            upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_plusInfinity);
+                        }
+
                     }
                 }
                 return true;
@@ -1342,26 +1355,11 @@ namespace iDynTree {
 
             virtual bool getVariablesLowerBound(VectorDynSize& variablesLowerBound) override {
 
-                bool stateBounded = true, controlBounded = true;
-
                 Eigen::Map<Eigen::VectorXd> stateBufferMap = toEigen(m_stateBuffer);
                 Eigen::Map<Eigen::VectorXd> controlBufferMap = toEigen(m_controlBuffer);
 
-                if (!(m_ocproblem->getStateLowerBound(m_stateBuffer))) {
-                    stateBounded = false;
-                    stateBufferMap.setConstant(m_minusInfinity);
-                }
-
-                if (!(m_ocproblem->getControlLowerBound(m_controlBuffer))) {
-                    controlBounded = false;
-                    controlBufferMap.setConstant(m_minusInfinity);
-                }
 
-                if (!controlBounded && !stateBounded) {
-                    return false;
-                }
-
-                if (variablesLowerBound.size() != numberOfVariables()) {
+                if (variablesLowerBound.size() != m_numberOfVariables) {
                     variablesLowerBound.resize(numberOfVariables());
                 }
                 Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(variablesLowerBound);
@@ -1370,15 +1368,43 @@ namespace iDynTree {
                 Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
 
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                bool isBounded;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
                         lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_minusInfinity); //avoiding setting bounds on the initial state
-                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+
+                        isBounded = m_ocproblem->getControlLowerBound(mesh->time, m_controlBuffer);
+                        if (isBounded) {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+                        } else {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu).setConstant(m_minusInfinity);
+                        }
+
                     } else if (mesh->type == MeshPointType::Control) {
-                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
-                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+
+                        isBounded = m_ocproblem->getControlLowerBound(mesh->time, m_controlBuffer);
+                        if (isBounded) {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
+                        } else {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu).setConstant(m_minusInfinity);
+                        }
+
+                        isBounded = m_ocproblem->getStateLowerBound(mesh->time, m_stateBuffer);
+                        if (isBounded) {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+                        } else {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_minusInfinity);
+                        }
+
                     } else if (mesh->type == MeshPointType::State) {
-                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+
+                        isBounded = m_ocproblem->getStateLowerBound(mesh->time, m_stateBuffer);
+                        if (isBounded) {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = stateBufferMap;
+                        } else {
+                            lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_minusInfinity);
+                        }
+
                     }
                 }
                 return true;
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 0541c9eb332..072d9307a92 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -75,7 +75,7 @@ namespace iDynTree {
             VectorDynSize costStateJacobianBuffer, costControlJacobianBuffer;
             MatrixDynSize costStateHessianBuffer, costControlHessianBuffer, costMixedHessianBuffer;
             bool stateLowerBounded, stateUpperBounded, controlLowerBounded, controlUpperBounded;
-            VectorDynSize stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
+            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
             SparsityStructure stateSparsity;
             SparsityStructure controlSparsity;
             std::vector<std::string> mayerCostnames;
@@ -662,8 +662,27 @@ namespace iDynTree {
                 reportError("OptimalControlProblem", "setStateLowerBound", "The dimension of minState does not coincide with the state dimension.");
                 return false;
             }
+            m_pimpl->stateLowerBounded = true;
+            m_pimpl->stateLowerBound = std::make_shared<TimeInvariantVector>(minState);
+
+            return true;
+        }
+
+        bool OptimalControlProblem::setStateLowerBound(std::shared_ptr<TimeVaryingVector> minState)
+        {
+            if (!(m_pimpl->dynamicalSystem)){
+                reportError("OptimalControlProblem", "setStateLowerBound", "First a dynamical system has to be set.");
+                return false;
+            }
+
+            if (!minState) {
+                reportError("OptimalControlProblem", "setStateLowerBound", "Empty minState pointer.");
+                return false;
+            }
+
             m_pimpl->stateLowerBounded = true;
             m_pimpl->stateLowerBound = minState;
+
             return true;
         }
 
@@ -678,8 +697,27 @@ namespace iDynTree {
                 reportError("OptimalControlProblem", "setStateUpperBound", "The dimension of maxState does not coincide with the state dimension.");
                 return false;
             }
+            m_pimpl->stateUpperBounded = true;
+            m_pimpl->stateUpperBound = std::make_shared<TimeInvariantVector>(maxState);
+
+            return true;
+        }
+
+        bool OptimalControlProblem::setStateUpperBound(std::shared_ptr<TimeVaryingVector> maxState)
+        {
+            if (!(m_pimpl->dynamicalSystem)){
+                reportError("OptimalControlProblem", "setStateUpperBound", "First a dynamical system has to be set.");
+                return false;
+            }
+
+            if (!maxState) {
+                reportError("OptimalControlProblem", "setStateUpperBound", "Empty maxState pointer.");
+                return false;
+            }
+
             m_pimpl->stateUpperBounded = true;
             m_pimpl->stateUpperBound = maxState;
+
             return true;
         }
 
@@ -694,8 +732,28 @@ namespace iDynTree {
                 reportError("OptimalControlProblem", "setControlLowerBound", "The dimension of minControl does not coincide with the control dimension.");
                 return false;
             }
+
+            m_pimpl->controlLowerBounded = true;
+            m_pimpl->controlLowerBound = std::make_shared<TimeInvariantVector>(minControl);
+
+            return true;
+        }
+
+        bool OptimalControlProblem::setControlLowerBound(std::shared_ptr<TimeVaryingVector> minControl)
+        {
+            if (!(m_pimpl->dynamicalSystem)){
+                reportError("OptimalControlProblem", "setControlLowerBound", "First a dynamical system has to be set.");
+                return false;
+            }
+
+            if (!minControl) {
+                reportError("OptimalControlProblem", "setControlLowerBound", "Empty minControl pointer.");
+                return false;
+            }
+
             m_pimpl->controlLowerBounded = true;
             m_pimpl->controlLowerBound = minControl;
+
             return true;
         }
 
@@ -710,8 +768,28 @@ namespace iDynTree {
                 reportError("OptimalControlProblem", "setControlUpperBound", "The dimension of maxControl does not coincide with the control dimension.");
                 return false;
             }
+
+            m_pimpl->controlUpperBounded = true;
+            m_pimpl->controlUpperBound = std::make_shared<TimeInvariantVector>(maxControl);
+
+            return true;
+        }
+
+        bool OptimalControlProblem::setControlUpperBound(std::shared_ptr<TimeVaryingVector> maxControl)
+        {
+            if (!(m_pimpl->dynamicalSystem)){
+                reportError("OptimalControlProblem", "setControlUpperBound", "First a dynamical system has to be set.");
+                return false;
+            }
+
+            if (!maxControl) {
+                reportError("OptimalControlProblem", "setControlUpperBound", "Empty maxControl pointer.");
+                return false;
+            }
+
             m_pimpl->controlUpperBounded = true;
             m_pimpl->controlUpperBound = maxControl;
+
             return true;
         }
 
@@ -720,42 +798,56 @@ namespace iDynTree {
             return setStateLowerBound(minState) && setStateUpperBound(maxState);
         }
 
+        bool OptimalControlProblem::setStateBoxConstraints(std::shared_ptr<TimeVaryingVector> minState, std::shared_ptr<TimeVaryingVector> maxState)
+        {
+            return setStateLowerBound(minState) && setStateUpperBound(maxState);
+        }
+
         bool OptimalControlProblem::setControlBoxConstraints(const VectorDynSize &minControl, const VectorDynSize &maxControl)
         {
             return setControlLowerBound(minControl) && setControlUpperBound(maxControl);
         }
 
-        bool OptimalControlProblem::getStateLowerBound(VectorDynSize &minState) const
+        bool OptimalControlProblem::setControlBoxConstraints(std::shared_ptr<TimeVaryingVector> minControl, std::shared_ptr<TimeVaryingVector> maxControl)
+        {
+            return setControlLowerBound(minControl) && setControlUpperBound(maxControl);
+        }
+
+        bool OptimalControlProblem::getStateLowerBound(double time, VectorDynSize &minState)
         {
             if (m_pimpl->stateLowerBounded){
-                minState = m_pimpl->stateLowerBound;
-                return true;
+                bool isValid = false;
+                minState = m_pimpl->stateLowerBound->get(time, isValid);
+                return isValid;
             }
             return false;
         }
 
-        bool OptimalControlProblem::getStateUpperBound(VectorDynSize &maxState) const
+        bool OptimalControlProblem::getStateUpperBound(double time, VectorDynSize &maxState)
         {
             if (m_pimpl->stateUpperBounded){
-                maxState = m_pimpl->stateUpperBound;
-                return true;
+                bool isValid = false;
+                maxState = m_pimpl->stateUpperBound->get(time, isValid);
+                return isValid;
             }
             return false;
         }
 
-        bool OptimalControlProblem::getControlLowerBound(VectorDynSize &minControl) const
+        bool OptimalControlProblem::getControlLowerBound(double time, VectorDynSize &minControl)
         {
             if (m_pimpl->controlLowerBounded){
-                minControl = m_pimpl->controlLowerBound;
+                bool isValid = false;
+                minControl = m_pimpl->controlLowerBound->get(time, isValid);
                 return true;
             }
             return false;
         }
 
-        bool OptimalControlProblem::getControlUpperBound(VectorDynSize &maxControl) const
+        bool OptimalControlProblem::getControlUpperBound(double time, VectorDynSize &maxControl)
         {
             if (m_pimpl->controlUpperBounded){
-                maxControl = m_pimpl->controlUpperBound;
+                bool isValid = false;
+                maxControl = m_pimpl->controlUpperBound->get(time, isValid);
                 return true;
             }
             return false;

From fb083c15814d472c2e03755bb7f3767e161b5423 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 26 Oct 2018 14:55:45 +0200
Subject: [PATCH 083/194] When setting the time range explicitly, it is no more
 AnyTime.

---
 src/optimalcontrol/src/TimeRange.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/optimalcontrol/src/TimeRange.cpp b/src/optimalcontrol/src/TimeRange.cpp
index 4828b5be06f..f8e12afcac6 100644
--- a/src/optimalcontrol/src/TimeRange.cpp
+++ b/src/optimalcontrol/src/TimeRange.cpp
@@ -62,6 +62,7 @@ namespace iDynTree {
 
             m_initTime = init;
             m_endTime = end;
+            m_anyTime = false;
 
             return true;
         }

From 2bd3fa48395587afbfdbb698d5a79920e6d6f18b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 29 Oct 2018 16:09:36 +0100
Subject: [PATCH 084/194] A TimeRange was seen as not valid even if it was an
 Instant.

---
 src/optimalcontrol/src/TimeRange.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/optimalcontrol/src/TimeRange.cpp b/src/optimalcontrol/src/TimeRange.cpp
index f8e12afcac6..d505b1a9646 100644
--- a/src/optimalcontrol/src/TimeRange.cpp
+++ b/src/optimalcontrol/src/TimeRange.cpp
@@ -126,7 +126,7 @@ namespace iDynTree {
 
         bool TimeRange::isValid() const
         {
-            return !(m_initTime >= m_endTime);
+            return (m_initTime <= m_endTime);
         }
 
         bool TimeRange::isInRange(double time) const

From fc2e046e201b51645a9269b52e37450371e22867 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 5 Nov 2018 15:44:40 +0100
Subject: [PATCH 085/194] Avoiding ambiguity in the addDenseBlock method.

---
 .../include/iDynTree/SparsityStructure.h      |  2 --
 src/optimalcontrol/src/SparsityStructure.cpp  | 34 +++----------------
 2 files changed, 4 insertions(+), 32 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
index 2c04ce3b6cb..898d47b10dd 100644
--- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -40,8 +40,6 @@ class iDynTree::optimalcontrol::SparsityStructure {
 
     void addDenseBlock(size_t startRow, size_t startColumn, size_t numberOfRows, size_t numberOfColumns);
 
-    bool addDenseBlock(long startRow, long startColumn, long numberOfRows, long numberOfColumns);
-
     bool addDenseBlock(const iDynTree::IndexRange& rowsRange, const iDynTree::IndexRange& columnsRange);
 
     void addIdentityBlock(size_t startRow, size_t startColumn, size_t dimension);
diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp
index 57972675165..111b666ea06 100644
--- a/src/optimalcontrol/src/SparsityStructure.cpp
+++ b/src/optimalcontrol/src/SparsityStructure.cpp
@@ -47,35 +47,6 @@ void iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(size_t startRow,
     }
 }
 
-bool iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(long startRow, long startColumn, long numberOfRows, long numberOfColumns)
-{
-    if (startRow < 0) {
-        reportError("SparsityStructure", "addDenseBlock", "The startRow is negative.");
-        return false;
-    }
-
-    if (startColumn < 0) {
-        reportError("SparsityStructure", "addDenseBlock", "The startColumn is negative.");
-        return false;
-    }
-
-    if (numberOfRows < 0) {
-        reportError("SparsityStructure", "addDenseBlock", "The numberOfRows is negative.");
-        return false;
-    }
-
-    if (numberOfColumns < 0) {
-        reportError("SparsityStructure", "addDenseBlock", "The numberOfColumns is negative.");
-        return false;
-    }
-
-    addDenseBlock(static_cast<size_t>(startRow),
-                  static_cast<size_t>(startColumn),
-                  static_cast<size_t>(numberOfRows),
-                  static_cast<size_t>(numberOfColumns));
-    return true;
-}
-
 bool iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(const iDynTree::IndexRange &rowsRange, const iDynTree::IndexRange &columnsRange)
 {
     if (!rowsRange.isValid()) {
@@ -88,7 +59,10 @@ bool iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(const iDynTree::
         return false;
     }
 
-    addDenseBlock(rowsRange.offset, columnsRange.offset, rowsRange.size, columnsRange.size);
+    addDenseBlock(static_cast<size_t>(rowsRange.offset),
+                  static_cast<size_t>(columnsRange.offset),
+                  static_cast<size_t>(rowsRange.size),
+                  static_cast<size_t>(columnsRange.size));
 
     return true;
 }

From 1c4bbd18518681d72a3ff14e436360e99d6f3dfc Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 7 Nov 2018 15:23:23 +0100
Subject: [PATCH 086/194] Avoid ambuigity in addIdentityBlock.

---
 .../include/iDynTree/SparsityStructure.h      |  2 --
 src/optimalcontrol/src/SparsityStructure.cpp  | 24 -------------------
 2 files changed, 26 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
index 898d47b10dd..aa0c63e09f9 100644
--- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -44,8 +44,6 @@ class iDynTree::optimalcontrol::SparsityStructure {
 
     void addIdentityBlock(size_t startRow, size_t startColumn, size_t dimension);
 
-    bool addIdentityBlock(long startRow, long startColumn, long dimension);
-
     void addNonZeroIfNotPresent(size_t newRow, size_t newCol);
 
     bool isValuePresent(size_t row, size_t col) const;
diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp
index 111b666ea06..8e7f1696e4b 100644
--- a/src/optimalcontrol/src/SparsityStructure.cpp
+++ b/src/optimalcontrol/src/SparsityStructure.cpp
@@ -74,30 +74,6 @@ void iDynTree::optimalcontrol::SparsityStructure::addIdentityBlock(size_t startR
     }
 }
 
-bool iDynTree::optimalcontrol::SparsityStructure::addIdentityBlock(long startRow, long startColumn, long dimension)
-{
-    if (startRow < 0) {
-        reportError("SparsityStructure", "addIdentityBlock", "The startRow is negative.");
-        return false;
-    }
-
-    if (startColumn < 0) {
-        reportError("SparsityStructure", "addIdentityBlock", "The startColumn is negative.");
-        return false;
-    }
-
-    if (dimension < 0) {
-        reportError("SparsityStructure", "addIdentityBlock", "The dimension is negative.");
-        return false;
-    }
-
-    addIdentityBlock(static_cast<size_t>(startRow),
-                     static_cast<size_t>(startColumn),
-                     static_cast<size_t>(dimension)
-                     );
-    return true;
-}
-
 void iDynTree::optimalcontrol::SparsityStructure::addNonZeroIfNotPresent(size_t newRow, size_t newCol)
 {
     if (!isValuePresent(newRow, newCol)) {

From 1e00acea32aa3fb0e7fffbaab5e9ddf0659f9f58 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 7 Nov 2018 15:23:54 +0100
Subject: [PATCH 087/194] Initializing to zero buffers for dynamics jacobian
 evaluation.

---
 src/optimalcontrol/src/ForwardEuler.cpp        | 2 ++
 src/optimalcontrol/src/ImplicitTrapezoidal.cpp | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index 82626b9d0fd..f0c4be4aebf 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -41,7 +41,9 @@ namespace iDynTree {
                 toEigen(m_identity) = iDynTreeEigenMatrix::Identity(nx, nx);
 
                 m_stateJacBuffer.resize(nx, nx);
+                m_stateJacBuffer.zero();
                 m_controlJacBuffer.resize(nx,nu);
+                m_controlJacBuffer.zero();
                 m_zeroBuffer.resize(nx,nu);
                 m_zeroBuffer.zero();
 
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index 6db2ea50ddd..d870495a924 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -43,7 +43,9 @@ namespace iDynTree {
                 toEigen(m_identity) = iDynTreeEigenMatrix::Identity(nx, nx);
 
                 m_stateJacBuffer.resize(nx, nx);
+                m_stateJacBuffer.zero();
                 m_controlJacBuffer.resize(nx,nu);
+                m_controlJacBuffer.zero();
 
                 m_stateJacobianSparsity.resize(2);
                 m_controlJacobianSparsity.resize(2);

From 7dbe1f4e84186449a751905101a594186cec1bed Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 12 Nov 2018 13:31:48 +0100
Subject: [PATCH 088/194] Removed duplication in the constraint evaluation in
 multiple shooting.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 061debc21f8..0227b5451cb 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -1474,8 +1474,6 @@ namespace iDynTree {
                 bool isValid = false;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
-                        guessMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) =
-                                toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
 
                         const iDynTree::VectorDynSize &controlGuess = m_controlGuesses->get(mesh->time, isValid);
 

From 95482ab2b2defbf744fc6882826b5a23cc724c72 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 13 Nov 2018 14:55:05 +0100
Subject: [PATCH 089/194] The constraints and the control jacobian were
 evaluated twice in case of single constraint.

---
 src/optimalcontrol/src/ConstraintsGroup.cpp | 33 +++++++++++++++++----
 1 file changed, 28 insertions(+), 5 deletions(-)

diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index 684a76a82f3..0bbbd4ce39e 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -313,6 +313,7 @@ namespace optimalcontrol {
                     reportError("ConstraintsGroup", "evaluateConstraints", errorMsg.str().c_str());
                     return false;
                 }
+                return true;
             }
 
             if (constraints.size() < m_pimpl->maxConstraintSize) {
@@ -401,7 +402,29 @@ namespace optimalcontrol {
         bool ConstraintsGroup::constraintJacobianWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &jacobian)
         {
             if (isAnyTimeGroup()) {
-                return m_pimpl->group.begin()->second.get()->constraint->constraintJacobianWRTState(time, state, control, jacobian);
+                TimedConstraint_ptr loneConstraint = m_pimpl->group.begin()->second;
+                if (!(loneConstraint->constraint->constraintJacobianWRTState(time, state, control, jacobian))) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Failed to evaluate "<< loneConstraint->constraint->name() << std::endl;
+                    reportError("ConstraintsGroup", "constraintJacobianWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (jacobian.rows() != loneConstraint->constraint->constraintSize()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state jacobian of constraint "<< loneConstraint->constraint->name() << " has a number of rows different from the size of the constraint." << std::endl;
+                    reportError("ConstraintsGroup", "constraintJacobianWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (jacobian.cols() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The state jacobian of constraint "<< loneConstraint->constraint->name() << " has a number of columns different from the state size." << std::endl;
+                    reportError("ConstraintsGroup", "constraintJacobianWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                return true;
             }
 
             if ((jacobian.rows() != m_pimpl->maxConstraintSize)||(jacobian.cols() != state.size())) {
@@ -455,28 +478,28 @@ namespace optimalcontrol {
         {
             if (isAnyTimeGroup()){
                 TimedConstraint_ptr loneConstraint = m_pimpl->group.begin()->second;
-                if (!(loneConstraint->constraint->constraintJacobianWRTControl(time, state, control, loneConstraint->controlJacobianBuffer))) {
+                if (!(loneConstraint->constraint->constraintJacobianWRTControl(time, state, control, jacobian))) {
                     std::ostringstream errorMsg;
                     errorMsg << "Failed to evaluate "<< loneConstraint->constraint->name() << std::endl;
                     reportError("ConstraintsGroup", "constraintJacobianWRTControl", errorMsg.str().c_str());
                     return false;
                 }
 
-                if (loneConstraint->controlJacobianBuffer.rows() != loneConstraint->constraint->constraintSize()) {
+                if (jacobian.rows() != loneConstraint->constraint->constraintSize()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The control jacobian of constraint "<< loneConstraint->constraint->name() << " has a number of rows different from the size of the constraint." << std::endl;
                     reportError("ConstraintsGroup", "constraintJacobianWRTControl", errorMsg.str().c_str());
                     return false;
                 }
 
-                if (loneConstraint->controlJacobianBuffer.cols() != control.size()) {
+                if (jacobian.cols() != control.size()) {
                     std::ostringstream errorMsg;
                     errorMsg << "The control jacobian of constraint "<< loneConstraint->constraint->name() << " has a number of columns different from the control size." << std::endl;
                     reportError("ConstraintsGroup", "constraintJacobianWRTControl", errorMsg.str().c_str());
                     return false;
                 }
 
-                jacobian = m_pimpl->group.begin()->second.get()->controlJacobianBuffer;
+                return true;
             }
 
             if ((jacobian.rows() != m_pimpl->maxConstraintSize)||(jacobian.cols() != control.size())) {

From 236d219f4629e29502e90b4e8ef8dbfaac2d9c4b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 14 Nov 2018 15:53:59 +0100
Subject: [PATCH 090/194] The constraint on the initial state is in the bounds.

Initialized constraints to avoid valgrind errors.
---
 src/optimalcontrol/src/IpoptInterface.cpp     |  1 +
 .../src/MultipleShootingSolver.cpp            | 24 ++++---------------
 2 files changed, 6 insertions(+), 19 deletions(-)

diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index a19a4e815e5..c24d1d28a42 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -108,6 +108,7 @@ namespace iDynTree {
 
                 if (constraintMultipliers.size() != numberOfConstraints) {
                     constraintMultipliers.resize(numberOfConstraints);
+                    constraintMultipliers.zero();
                 }
 
                 if ((m_jacobianBuffer.rows() != numberOfConstraints) || (m_jacobianBuffer.cols() != numberOfVariables)) {
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 0227b5451cb..4f1eba6b7e6 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -991,7 +991,7 @@ namespace iDynTree {
 
                 m_constraintsPerInstant = m_ocproblem->getConstraintsDimension();
                 size_t nc = m_constraintsPerInstant;
-                m_numberOfConstraints = (m_totalMeshes) * nx + (m_constraintsPerInstant) * (m_totalMeshes); //dynamical constraints (plus identity constraint for the initial state) and normal constraints
+                m_numberOfConstraints = (m_totalMeshes - 1) * nx + (m_constraintsPerInstant) * (m_totalMeshes); //dynamical constraints (plus identity constraint for the initial state) and normal constraints
 
                 //Determine problem type
                 m_infoData->hasLinearConstraints = (m_ocproblem->countLinearConstraints() != 0) || m_ocproblem->systemIsLinear();
@@ -1040,11 +1040,6 @@ namespace iDynTree {
                         index += nu;
                         previousControlMesh = mesh;
 
-                        lowerBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)) = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                        upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)) = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
-                        addIdentityJacobianBlock(constraintIndex, mesh->stateIndex, nx);
-                        constraintIndex += nx;
-
                         //Saving constraints bounds
                         if (!(m_ocproblem->getConstraintsLowerBound(mesh->time, m_minusInfinity, m_constraintsBuffer))){
                             std::ostringstream errorMsg;
@@ -1314,7 +1309,7 @@ namespace iDynTree {
                 bool isBounded;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
-                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_plusInfinity); //avoiding setting bounds on the initial state
+                        upperBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
 
                         isBounded = m_ocproblem->getControlUpperBound(mesh->time, m_controlBuffer);
                         if (isBounded) {
@@ -1371,8 +1366,7 @@ namespace iDynTree {
                 bool isBounded;
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
                     if (mesh->origin == first){
-                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx).setConstant(m_minusInfinity); //avoiding setting bounds on the initial state
-
+                        lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx) = toEigen(m_ocproblem->dynamicalSystem().lock()->initialState());
                         isBounded = m_ocproblem->getControlLowerBound(mesh->time, m_controlBuffer);
                         if (isBounded) {
                             lowerBoundMap.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu) = controlBufferMap;
@@ -1733,10 +1727,7 @@ namespace iDynTree {
                     currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
                     previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
 
-                    if (mesh->origin == first) {
-                        constraintsMap.segment(constraintIndex, nx) = currentState; //identity constraint for the initial state
-                        constraintIndex += nx;
-                    } else {
+                    if (mesh->origin != first) {
                         previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
                         dT = mesh->time - (mesh - 1)->time;
                         if (!(m_integrator->evaluateCollocationConstraint(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT, m_stateBuffer))){
@@ -1802,12 +1793,7 @@ namespace iDynTree {
                     currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
                     previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
 
-                    if (mesh->origin == first) {
-
-                        jacobianMap.block(constraintIndex, static_cast<Eigen::Index>(mesh->stateIndex), nx, nx).setIdentity();
-                        constraintIndex += nx;
-
-                    } else {
+                    if (mesh->origin != first) {
                         previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
                         dT = mesh->time - (mesh - 1)->time;
                         if (!(m_integrator->evaluateCollocationConstraintJacobian(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT, m_collocationStateJacBuffer, m_collocationControlJacBuffer))){

From 62c2e9d68293a7dbee690f1ceaa601b1af76df06 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 22 Jan 2019 09:16:02 +0100
Subject: [PATCH 091/194] Updated release notes.

---
 doc/releases/v0_12.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 5dcf7711164..259c753bacf 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -26,7 +26,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Fixed bugs in ``MultipleShooting`` solver.
 * Added few lines of documentation.
 * Added interface for ``ALGLIB`` and ``WORHP``.
+* Taking into account also the sparsity pattern of constraints and dynamical system.
 
 #### `visualization`
 * Added visualization of vectors.
-

From c777beacd5e04998e1d083a1640029afc958c840 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <pegua1@gmail.com>
Date: Tue, 22 Jan 2019 10:25:08 +0100
Subject: [PATCH 092/194] Applied suggestions from code review

Co-Authored-By: S-Dafarra <stefano.dafarra@gmail.com>
---
 src/optimalcontrol/tests/MultipleShootingTest.cpp | 2 +-
 src/optimalcontrol/tests/OCProblemTest.cpp        | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 07bb6000710..e49d1e28d37 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -78,7 +78,7 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
         return true;
     }
 
-    virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override{
+    virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override {
         iDynTree::optimalcontrol::SparsityStructure sparsity;
         sparsity.addIdentityBlock(0ul, 0, 2);
         stateSparsity = sparsity;
diff --git a/src/optimalcontrol/tests/OCProblemTest.cpp b/src/optimalcontrol/tests/OCProblemTest.cpp
index 45c9591ea16..8717e64e47a 100644
--- a/src/optimalcontrol/tests/OCProblemTest.cpp
+++ b/src/optimalcontrol/tests/OCProblemTest.cpp
@@ -74,7 +74,7 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
         return true;
     }
 
-    virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override{
+    virtual bool dynamicsStateFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) override {
         iDynTree::optimalcontrol::SparsityStructure sparsity;
         sparsity.addIdentityBlock(0ul, 0, 2);
         stateSparsity = sparsity;

From 27c1008229f18d1adba528ca4199f9f3cffb6122 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 22 Jan 2019 10:29:26 +0100
Subject: [PATCH 093/194] Using big number for patch version.

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5f7808e46ce..5f31c4a130f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,7 +25,7 @@ set(VARS_PREFIX "iDynTree")
 
 set(${VARS_PREFIX}_MAJOR_VERSION 0)
 set(${VARS_PREFIX}_MINOR_VERSION 11)
-set(${VARS_PREFIX}_PATCH_VERSION 2)
+set(${VARS_PREFIX}_PATCH_VERSION 102)
 set(${VARS_PREFIX}_VERSION ${${VARS_PREFIX}_MAJOR_VERSION}.${${VARS_PREFIX}_MINOR_VERSION}.${${VARS_PREFIX}_PATCH_VERSION})
 
 # Pick up our CMake scripts - they are all in the cmake subdirectory.

From 1829e1dc3d90a8f6fb289bfb0ab5a16e8d9adc9b Mon Sep 17 00:00:00 2001
From: Prashanth Ramadoss <prashanthramadoss@gmail.com>
Date: Tue, 29 Jan 2019 14:32:56 +0100
Subject: [PATCH 094/194] Estimation/attitude estimator (#516)

---
 doc/releases/v0_12.md                         |   7 +
 src/estimation/CMakeLists.txt                 |  15 +-
 .../iDynTree/Estimation/AttitudeEstimator.h   | 181 +++++
 .../Estimation/AttitudeEstimatorUtils.h       | 115 ++++
 .../Estimation/AttitudeMahonyFilter.h         | 175 +++++
 .../Estimation/AttitudeQuaternionEKF.h        | 344 +++++++++
 .../Estimation/ExtendedKalmanFilter.h         | 282 ++++++++
 src/estimation/src/AttitudeEstimator.cpp      |  16 +
 src/estimation/src/AttitudeEstimatorUtils.cpp | 138 ++++
 src/estimation/src/AttitudeMahonyFilter.cpp   | 263 +++++++
 src/estimation/src/AttitudeQuaternionEKF.cpp  | 651 ++++++++++++++++++
 src/estimation/src/ExtendedKalmanFilter.cpp   | 286 ++++++++
 .../tests/AttitudeEstimatorUnitTest.cpp       |  84 +++
 src/estimation/tests/CMakeLists.txt           |   1 +
 14 files changed, 2555 insertions(+), 3 deletions(-)
 create mode 100644 src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h
 create mode 100644 src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
 create mode 100644 src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
 create mode 100644 src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
 create mode 100644 src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
 create mode 100644 src/estimation/src/AttitudeEstimator.cpp
 create mode 100644 src/estimation/src/AttitudeEstimatorUtils.cpp
 create mode 100644 src/estimation/src/AttitudeMahonyFilter.cpp
 create mode 100644 src/estimation/src/AttitudeQuaternionEKF.cpp
 create mode 100644 src/estimation/src/ExtendedKalmanFilter.cpp
 create mode 100644 src/estimation/tests/AttitudeEstimatorUnitTest.cpp

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 259c753bacf..22de63243b1 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -30,3 +30,10 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 
 #### `visualization`
 * Added visualization of vectors.
+
+#### `estimation`
+* Added attitude estimator interface to estimate the orientation of an IMU, given the IMU measurements
+* Added `DiscreteExtendedKalmanFilterHelper` base class
+* Added `AttitudeMahonyFilter` implementation of an explicit formulation of passive complementary filter over quaternion groups
+* Added `AttitudeQuaternionEKF` implementation
+* All of the above addressed in the PR (https://github.com/robotology/idyntree/pull/516)
diff --git a/src/estimation/CMakeLists.txt b/src/estimation/CMakeLists.txt
index cbe660cc58a..07d35a809b2 100644
--- a/src/estimation/CMakeLists.txt
+++ b/src/estimation/CMakeLists.txt
@@ -16,9 +16,13 @@ set(IDYNTREE_ESTIMATION_HEADERS include/iDynTree/Estimation/BerdyHelper.h
                                 include/iDynTree/Estimation/SchmittTrigger.h
                                 include/iDynTree/Estimation/ContactStateMachine.h
                                 include/iDynTree/Estimation/BipedFootContactClassifier.h
-                                include/iDynTree/Estimation/GravityCompensationHelpers.h)
+                                include/iDynTree/Estimation/GravityCompensationHelpers.h
+                                include/iDynTree/Estimation/ExtendedKalmanFilter.h
+                                include/iDynTree/Estimation/AttitudeEstimator.h
+                                include/iDynTree/Estimation/AttitudeMahonyFilter.h
+                                include/iDynTree/Estimation/AttitudeQuaternionEKF.h)
 
-set(IDYNTREE_ESTIMATION_PRIVATE_INCLUDES )
+set(IDYNTREE_ESTIMATION_PRIVATE_INCLUDES include/iDynTree/Estimation/AttitudeEstimatorUtils.h)
 
 set(IDYNTREE_ESTIMATION_SOURCES src/BerdyHelper.cpp
                                 src/ExternalWrenchesEstimation.cpp
@@ -28,7 +32,12 @@ set(IDYNTREE_ESTIMATION_SOURCES src/BerdyHelper.cpp
                                 src/SchmittTrigger.cpp
                                 src/ContactStateMachine.cpp
                                 src/BipedFootContactClassifier.cpp
-                                src/GravityCompensationHelpers.cpp)
+                                src/GravityCompensationHelpers.cpp
+                                src/ExtendedKalmanFilter.cpp
+                                src/AttitudeEstimator.cpp
+                                src/AttitudeEstimatorUtils.cpp
+                                src/AttitudeMahonyFilter.cpp
+                                src/AttitudeQuaternionEKF.cpp)
 
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_ESTIMATION_SOURCES})
 SOURCE_GROUP("Header Files" FILES ${IDYNTREE_ESTIMATION_HEADERS})
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h
new file mode 100644
index 00000000000..bb21d795ef5
--- /dev/null
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h
@@ -0,0 +1,181 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef IDYNTREE_ATTITUDEESTIMATOR_H
+#define IDYNTREE_ATTITUDEESTIMATOR_H
+
+#include <iDynTree/Core/VectorFixSize.h>
+#include <iDynTree/Core/Rotation.h>
+
+#include <iostream>
+
+namespace iDynTree
+{
+    typedef iDynTree::Vector3 LinearAccelerometerMeasurements;
+    typedef iDynTree::Vector3 GyroscopeMeasurements;
+    typedef iDynTree::Vector3 MagnetometerMeasurements;
+
+    typedef iDynTree::Vector4 UnitQuaternion;
+    typedef iDynTree::Vector3 RPY;
+
+    /**
+     * @class IAttitudeEstimator generic interface for attitude estimator classes
+     *
+     * The aim is to implement different attitude estimators as a block that takes IMU measurements
+     * as inputs and gives attitude estimates as outputs. This way the underlying implementation is abstracted
+     * and the user only has to set a few parameters and run the estimator.
+     *
+     * The general procedure to use the estimators would be,
+     * - instantiate the filter,
+     * - set initial internal state
+     * - in a loop,
+     *     - update the filter with measurements
+     *     - propagate the states
+     *
+     * However, additional methods to set and get parameters for the filter might be available with respect to the filters.
+     *
+     * The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$
+     * \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+     * \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$  is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and
+     * \f$ {^B}b \in \mathbb{R}^3 \f$  is the gyroscope bias expressed in the body frame.
+     *
+     */
+    class IAttitudeEstimator
+    {
+    public:
+        virtual ~IAttitudeEstimator();
+
+        /**
+         * @brief Update the filter with accelerometer and gyroscope measurements
+         *
+         * @param[in] linAccMeas proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B
+         * @param[in] gyroMeas angular velocity of body frame B with respect to an inertial fram A, expressed in frame B
+         *
+         * @note consider the current behavior of our system does not use magnetometer measurements and is calling this method to update measurements.
+         *       Then, if we decide to turn the flag use_magnetometer_measurements to true, this will not guarantee that the magnetometer measurements
+         *       will be used. The magnetometer measurements will be used only if we replace this function call with the other overlaoded function considering
+         *       the magnetometer measurements.
+         *
+         * @return true/false if successful/not
+         */
+        virtual bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
+                                                  const iDynTree::GyroscopeMeasurements& gyroMeas) = 0;
+
+        /**
+         * @brief Update the filter with accelerometer, gyroscope and magnetometer measurements
+         *
+         * @param[in] linAccMeas proper (body acceleration - gravity) classical acceleration of the origin of the body frame B expressed in frame B
+         * @param[in] gyroMeas angular velocity of body frame B with respect to an inertial fram A, expressed in frame B
+         * @param[in] magMeas magnetometer measurements expressed in frame B
+         *
+         * @return true/false if successful/not
+         */
+        virtual bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
+                                                  const iDynTree::GyroscopeMeasurements& gyroMeas,
+                                                  const iDynTree::MagnetometerMeasurements& magMeas) = 0;
+
+        /**
+         * @brief Propagate the states and associated uncertainties through properly defined propagation functions
+         * The underlying implementation depends on the type of filter being implemented.
+         *
+         * @return true/false if successful/not
+         */
+        virtual bool propagateStates() = 0;
+
+        /**
+         * @brief Get orientation of the body with respect to inertial frame, in rotation matrix form
+         *        If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body,
+         *        then this method gives us \f$ {^A}R_B \f$ as the rotation matrix
+         * @param[out] rot Rotation matrix
+         * @return true/false if successful/not
+         */
+        virtual bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) = 0;
+
+        /**
+         * @brief Get orientation of the body with respect to inertial frame, in unit quaternion form
+         *        If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body,
+         *        then this method gives us \f$ {^A}q_B as the quaternion \f$
+         *
+         * @note quaternion has the form (real, imaginary) and is normalized
+         * @note Usually a rotation matrix can be described using two quaternions due to its double-connectedness property
+         *       Depending on the specific filter, the initial state and the trajectory of the system, we could obtain
+         *       one quaternion or the other(opposite spin), depending on the system dynamics.
+         *
+         * @param[out] q UnitQuaternion
+         * @return true/false if successful/not
+         */
+        virtual bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) = 0;
+
+        /**
+         * @brief Get orientation of the body with respect to inertial frame, in Euler's RPY form
+         *        If we denote \f$ A \f$ as inertial frame and \f$ B \f$ as the frame attached to the body,
+         *        then this method gives us the RPY 3d vector of Euler Angles when composed together gives us \f$ {^A}R_B \f$ as the rotation matrix
+         *        where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll)\f$.
+         *        For more details about the range of the RPY Euler angles, please refer the documentation of
+         *   <a href="/robotology/idyntree/blob/c8bf721b771fa4b1e7c3a940632e121060719a19/src/core/include/iDynTree/Core/Rotation.h#L167">GetRPY()</a>
+         *
+         * @param[out] rpy 3D vector containing roll pitch yaw angles
+         * @return true/false if successful/not
+         */
+        virtual bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) = 0;
+
+        /**
+         * @brief Get dimension of the state vector
+         * @return size_t size of state vector
+         */
+        virtual size_t getInternalStateSize() const = 0;
+
+        /**
+         * @brief Get internal state of the estimator
+         *        The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$
+         *        \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+         *        \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$  is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and
+         *        \f$ {^B}b \in \mathbb{R}^3 \f$  is the gyroscope bias expressed in the body frame.
+         *        The default internal state of the estimator would be \f$ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T \f$
+         * @param[out] stateBuffer Span object as reference of the container where state vector should be copied to
+         * @return true/false if successful/not
+         */
+        virtual bool getInternalState(const iDynTree::Span<double> & stateBuffer) const = 0;
+
+        /**
+         * @brief Get initial internal state of the estimator
+         *        The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$
+         *        \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+         *        \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$  is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and
+         *        \f$ {^B}b \in \mathbb{R}^3 \f$  is the gyroscope bias expressed in the body frame.
+         *        The default internal state of the estimator would be \f$ X = \begin{bmatrix} 1.0 \\ 0_{1 \times 3} \\ 0_{1 \times 3} \\ 0_{1 \times 3} \end{bmatrix}^T \f$
+         * @param[out] stateBuffer Span object as reference of the container where state vector should be copied to
+         * @return true/false if successful/not
+         */
+        virtual bool getDefaultInternalInitialState(const iDynTree::Span<double> & stateBuffer) const = 0;
+
+        /**
+         * @brief set internal state of the estimator.
+         *        The internal state of the estimator is described as \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$
+         *        \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+         *        \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$  is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and
+         *        \f$ {^B}b \in \mathbb{R}^3 \f$  is the gyroscope bias expressed in the body frame.
+         * @param[in] stateBuffer Span object as reference of the container from which the internal state vector should be assigned. The size of the buffer should be 10.
+         * @return true/false if successful/not
+         */
+        virtual bool setInternalState(const iDynTree::Span<double> & stateBuffer) = 0;
+
+        /**
+         * @brief set the initial orientation for the internal state of the estimator.
+         *        The initial orientation for the internal state of the estimator is described as \f$ {^A}q_B \f$
+         *        \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+         * @param[in] stateBuffer Span object as reference of the container from which the inital orientaiton for internal state vector should be assigned. The size of the buffer should be 4.
+         * @return true/false if successful/not
+         */
+        virtual bool setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) = 0;
+    };
+
+}
+#endif
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
new file mode 100644
index 00000000000..1b4b684dfea
--- /dev/null
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef IDYNTREE_ATTITUDE_ESTIMATOR_UTILS_H
+#define IDYNTREE_ATTITUDE_ESTIMATOR_UTILS_H
+
+#include <iDynTree/Estimation/AttitudeEstimator.h>
+#include <iDynTree/Core/EigenHelpers.h>
+#define _USE_MATH_DEFINES
+#include <cmath>
+#include <vector>
+
+/**
+ *
+ * @brief computes the cross vector of two 3D vectors
+ * @param[in] a 3D vector
+ * @param[in] b 3D vector
+ * @return iDynTree::Vector3
+ */
+iDynTree::Vector3 crossVector(const iDynTree::Vector3& a, const iDynTree::Vector3& b);
+
+/**
+ * @brief computes \f$ 3 \times 3 \f$ skew-symmetric matrix (\f$ \mathbb{so}(3) \$ space) for a given 3d vector (\f$ \mathbb{R}^3 \f$ space)
+ *
+ * @param[in] omega 3d vector (usually angular velocity)
+ * @return iDynTree::Matrix3x3
+ */
+iDynTree::Matrix3x3 mapR3Toso3(const iDynTree::Vector3& omega);
+
+/**
+ * @brief checks if the \f$ 3 \times 3 \f$ matrix is skew-symmetric
+ *        \f[ S + S^T = 0 \f]
+ * @param[in] S \f$ 3 \times 3 \f$ matrix
+ * @return bool true/false
+ */
+bool checkSkewSymmetricity(const iDynTree::Matrix3x3& S);
+
+/**
+ * @brief computes 3D vector (\f$ \mathbb{R}^3 \f$ space) from a skew symmetric matrix (\f$ \mathbb{so}(3) \$ space)
+ *
+ * @param[in] S \f$ 3 \times 3 \f$ matrix
+ * @return iDynTree::Vector3
+ */
+iDynTree::Vector3 mapso3ToR3(const iDynTree::Matrix3x3& S);
+
+/**
+ * @brief computes scalar dot product of two 3-d vectors
+ *        Maps  (\f$ \mathbb{R}^n \f$ space) to (\f$ \mathbb{R} \f$ space) through its dual vector
+ * @param[in] a 3D vector (not passed as reference, but as a copy to avoid changes in source due to in-place manipulation)
+ * @param[in] b 3D vector
+ * @return double
+ */
+double innerProduct(const iDynTree::Vector3 a, const iDynTree::Vector3& b);
+
+/**
+ * @brief real part of quaternion,
+ *          \f$ s \f$ in \f$ s + i v_1 + i v_2 + i v_3 \f$
+ *
+ * @param[in] q 4d vector or quaternion
+ * @return double
+ */
+double realPartOfQuaternion(const iDynTree::UnitQuaternion& q);
+
+/**
+ * @brief imaginary part of quaternion
+ *        \f$ v \f$ in \f$ s + i v_1 + i v_2 + i v_3 \f$
+ * @param[in] q 4d vector or quaternion
+ * @return iDynTree::Vector3
+ */
+iDynTree::Vector3 imaginaryPartOfQuaternion(const iDynTree::UnitQuaternion& q);
+
+/**
+ * @brief composition operator - quaternion multiplication
+ *
+ * @param[in] q1 4d vector or quaternion
+ * @param[in] q2 4d vector or quaternion
+ * @return iDynTree::UnitQuaternion
+ */
+iDynTree::UnitQuaternion composeQuaternion(const iDynTree::UnitQuaternion& q1, const iDynTree::UnitQuaternion& q2);
+
+/**
+ * @brief computes the matrix map of quaternion left multiplication \f$ q1 \circ q2 = q1q2 \f$
+ *        in opposition to the right multiplication \f$ q1 \circ q2 = q2q1 \f$
+ * @param[in] x 4d vector or quaternion q1
+ * @return iDynTree::Matrix4x4
+ */
+iDynTree::Matrix4x4 mapofYQuaternionToXYQuaternion(const iDynTree::UnitQuaternion &x);
+
+
+/**
+ * @brief composition operator - quaternion multiplication
+ *        this method is faster than composeQuaternion()
+ * @param[in] q1 4d vector or quaternion
+ * @param[in] q2 4d vector or quaternion
+ * @return iDynTree::UnitQuaternion
+ */
+iDynTree::UnitQuaternion composeQuaternion2(const iDynTree::UnitQuaternion &q1, const iDynTree::UnitQuaternion &q2);
+
+
+/**
+ * @brief computes pure quaternion given a 3d vector (uually angular velocity)
+ *
+ * @param[in] bodyFixedFrameVelocityInInertialFrame 3d vector
+ * @return iDynTree::UnitQuaternion
+ */
+iDynTree::UnitQuaternion pureQuaternion(const iDynTree::Vector3& bodyFixedFrameVelocityInInertialFrame);
+
+#endif
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
new file mode 100644
index 00000000000..113c40a8862
--- /dev/null
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
@@ -0,0 +1,175 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef ATTITUDE_MAHONY_FILTER_H
+#define ATTITUDE_MAHONY_FILTER_H
+
+#include <iDynTree/Estimation/AttitudeEstimator.h>
+#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
+#include <iDynTree/Core/Direction.h>
+
+namespace iDynTree
+{
+
+/**
+* @struct AttitudeMahonyFilterParameters Parameters to set up the quaternion EKF
+* @var AttitudeMahonyFilterParameters::time_step_in_seconds
+* discretization time step in seconds, default value: \f$ 0.001 s \f$
+* @var AttitudeMahonyFilterParameters::kp
+* Mahony \f$ K_p \f$ gain over the correction from IMU measurements, default value: \f$ 1.0 \f$
+* @var AttitudeMahonyFilterParameters::ki
+* Mahony \f$ K_i \f$ gain over the gyro bias evolution, default value: \f$ 1.0 \f$
+* @var AttitudeMahonyFilterParameters::use_magenetometer_measurements
+* flag to enable the use of magnetometer measurement for yaw correction, default value: false
+* @var AttitudeMahonyFilterParameters::confidence_magnetometer_measurements
+* confidence on magnetometer measurements, default value: \f$ 0.0 \f$
+*/
+struct AttitudeMahonyFilterParameters {
+    double time_step_in_seconds{0.001};
+    double kp{1.0};
+    double ki{1.0};
+    bool use_magnetometer_measurements{false};
+    double confidence_magnetometer_measurements{0.0};
+};
+
+
+/**
+ * @class AttitudeMahonyFilter Implements an explicit passive complementary filter on quaternion groups
+ * described in the paper <a href="https://hal.archives-ouvertes.fr/hal-00488376/document">Non-linear complementary filters on SO3 groups</a>
+ *
+ * The filter is used to estimate the states \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$
+ * where \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+ *       \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$  is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and
+ *       \f$ {^B}b \in \mathbb{R}^3 \f$  is the gyroscope bias expressed in the body frame.
+ * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience
+ *
+ * The discretized dynamics of the filter is implemented in the propagateStates() method and is described by the following equations,
+ * \f$ q_{k+1} = q_{k} + \Delta t \frac{1}{2}q_{k} \circ \begin{bmatrix} 0 \\ \Omega_y_{k+1} - b_k + K_p \omega_{mes_{k+1}}\end{bmatrix}\f$
+ * \f$ \Omega_{k+1} = \Omega_y_{k+1} - b_k \f$
+ * \f$ b_{k+1} = b_k - K_i \Delta t \frac{1}{2} \omega_{mes_{k+1}} \f$
+ *
+ * The updateFilterWithMeasurements() uses the recent IMU measurements to compute the term \f$ \omega_{mes} \f$ which gives the vectorial from accelerometer and magnetometer measurements
+ * \f$ \omega_{mes} = -(\Sigma{n}{i=1} \frac{k_i}{2} (v_i \hat{v}_i^T - \hat{v}_i v_i^T) )^{\vee} \f$
+ * where \f$ v_i \f$ is the normalized accelerometer or magnetometer measurement,
+ * \f$ \hat{v_i} \f$ is the vector obtained from the orientation estimated combined with gravity direction or absolute magnetic field direction, for e.g, \f$ \hat{v_acc} = {^w}R_b^T e_3 \f$
+ * and \f$ k_i \f$ is the confidence weight on the i-th measurement. In our case, i = 1 or 2.
+ *
+ * The usage of the attitude estimator can be as follows,
+ * - After instantiation, the parameters of the filter can be set using the individual parameter methods or the struct method.
+ * - The filter state can be initialized by calling the setInternalState() method
+ * - Once initialized, the following filter methods can be run in a loop to get the orientation estimates,
+ *     - updateFilterWithMeasurements() method to pass the recent measurements to the filter
+ *     - propagateStates() method to propagate the states through the system dynamics and correcting using the updated measurements
+ *     - getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation
+ */
+class AttitudeMahonyFilter : public IAttitudeEstimator
+{
+public:
+    AttitudeMahonyFilter();
+
+    /**
+     * @brief set flag to use magnetometer measurements
+     * @param[in] flag enable/disable magnetometer measurements
+     */
+    void useMagnetoMeterMeasurements(bool flag);
+
+    /**
+     * @brief set the confidence weights on magenetometer measurements, if used
+     * @param[in] confidence can take values between \f$ [0, 1] \f$
+     */
+    void setConfidenceForMagnetometerMeasurements(double confidence);
+
+    /**
+     * @brief set the Kp gain
+     * @param[in] kp gain
+     */
+    void setGainkp(double kp);
+
+    /**
+     * @brief set the Ki gain
+     * @param[in] ki gain
+     */
+    void setGainki(double ki);
+
+    /**
+     * @brief set discretization time step in seconds
+     * @param[in] timestepInSeconds time step
+     */
+    void setTimeStepInSeconds(double timestepInSeconds);
+
+    /**
+     * @brief Set the gravity direction assumed by the filter (for computing orientation vectorial from accelerometer)
+     * @param[in] gravity_dir gravity direction
+     */
+    void setGravityDirection(const iDynTree::Direction& gravity_dir);
+
+    /**
+     * @brief Set filter parameters with the struct members.
+     * This does not reset the internal state.
+     * @param[in] params object of AttitudeMahonyFilterParameters passed as a const reference
+     * @return true/false if successful/not
+     */
+    bool setParameters(const AttitudeMahonyFilterParameters& params)
+    {
+        m_params = params;
+        return true;
+    }
+
+    /**
+     * @brief Get filter parameters as a struct.
+     * @param[out] params object of AttitudeMahonyFilterParameters passed as reference
+     */
+    void getParameters(AttitudeMahonyFilterParameters& params) {params = m_params;}
+
+    bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
+                                      const iDynTree::GyroscopeMeasurements& gyroMeas) override;
+    bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
+                                      const iDynTree::GyroscopeMeasurements& gyroMeas,
+                                      const iDynTree::MagnetometerMeasurements& magMeas) override;
+    bool propagateStates() override;
+    bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override;
+    bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override;
+    bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override;
+    size_t getInternalStateSize() const override;
+    bool getInternalState(const iDynTree::Span<double> & stateBuffer) const override;
+    bool getDefaultInternalInitialState(const iDynTree::Span<double> & stateBuffer) const override;
+    bool setInternalState(const iDynTree::Span<double> & stateBuffer) override;
+    bool setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) override;
+
+private:
+    AttitudeMahonyFilterParameters m_params;              ///< struct holding the Mahony filter parameters
+
+    /** @struct state internal state of the estimator
+     * @var state::m_orientation
+     * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation
+     * @var state::m_orientation
+     * angular velocity estimate in \f$ \mathbb{R}^3 \f$
+     * @var state::m_orientation
+     * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$
+     */
+    struct {
+        iDynTree::UnitQuaternion m_orientation;
+        iDynTree::Vector3 m_angular_velocity;
+        iDynTree::Vector3 m_gyroscope_bias;
+    } m_state, m_initial_state;
+
+    iDynTree::Rotation m_orientationInSO3;                ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body
+    iDynTree::RPY m_orientationInRPY;                     ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$
+
+    iDynTree::Vector3 m_omega_mes;                        ///< vectorial estimate from accelerometer and magnetometer measurements, \f$ \omega_{mes} \in \mathbb{R}^3 \f$, notation from the paper Non linear complementary filters on the special orthogonal group
+    iDynTree::GyroscopeMeasurements m_Omega_y;            ///< gyroscope measurement, \f$ \Omega_{y} \in \mathbb{R}^3 \f$, notation from the paper Non linear complementary filters on the special orthogonal group
+
+    iDynTree::Direction m_gravity_direction;              ///< direction of the gravity vector expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ e_3 = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$
+    iDynTree::Direction m_earth_magnetic_field_direction; ///< direction of absolute magnetic field expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ {^A}m = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$
+};
+
+}
+
+#endif
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
new file mode 100644
index 00000000000..7543ba663e0
--- /dev/null
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
@@ -0,0 +1,344 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef ATTITUDE_QUATERNION_EKF_H
+#define ATTITUDE_QUATERNION_EKF_H
+
+#include <iDynTree/Estimation/AttitudeEstimator.h>
+#include <iDynTree/Estimation/ExtendedKalmanFilter.h>
+#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
+#include <iDynTree/Core/Direction.h>
+
+namespace iDynTree
+{
+    const unsigned int output_dimensions_with_magnetometer = 4;        ///< dimension of \f$ \mathbb{R}^3 \times \mathbb{R} \f$ accelerometer measurements and magnetometer yaw measurement
+    const unsigned int output_dimensions_without_magnetometer = 3;     ///< dimension of \f$ \mathbb{R}^3 \f$ accelerometer measurements
+    const unsigned int input_dimensions = 3;                           ///< dimension of \f$ \mathbb{R}^3 \f$ gyroscope measurements
+
+    /**
+     * @struct AttitudeQuaternionEKFParameters Parameters to set up the quaternion EKF
+     * @var AttitudeQuaternionEKFParameters::time_step_in_seconds
+     * discretization time step in seconds, default value: \f$ 0.01 s \f$
+     * @var AttitudeQuaternionEKFParameters::bias_correlation_time_factor
+     * time factor modeling how fast the bias can vary, default value: \f$ 0.01 \f$
+     * @var AttitudeQuaternionEKFParameters::accelerometer_noise_variance
+     * measurement noise covariance \f$ \sigma_{acc}^{2} \f$ of accelerometer measurement, default value: \f$ 0.001 \f$
+     * @var AttitudeQuaternionEKFParameters::magnetometer_noise_variance
+     * measurement noise covariance \f$ \sigma_{mag}^{2} \f$ of magnetometer measurement, default value: \f$ 0.001 \f$
+     * @var AttitudeQuaternionEKFParameters::gyroscope_noise_variance
+     * system noise covariance \f$ \sigma_{gyro}^{2} \f$ of gyroscope measurement, since it is the input to the system, default value: \f$ 0.001 \f$
+     * @var AttitudeQuaternionEKFParameters::gyro_bias_noise_variance
+     * system noise covariance \f$ \sigma_{gyrobias}^{2} \f$ of gyroscope bias estimate, default value: \f$ 0.0001 \f$
+     * @var AttitudeQuaternionEKFParameters::initial_orientation_error_variance
+     * initial state covariance \f$ \sigma_{q_0}^{2} \f$ of orientation, default value: \f$ 10 \f$
+     * @var AttitudeQuaternionEKFParameters::initial_ang_vel_error_variance
+     * initial state covariance \f$ \sigma_{\omega_0}^{2} \f$ of angular velocity, default value: \f$ 10 \f$
+     * @var AttitudeQuaternionEKFParameters::initial_gyro_bias_error_variance
+     * measurement noise covariance \f$ \sigma_{acc}^{2} \f$ of gyroscope bias, default value: \f$ 10 \f$
+     * @var AttitudeQuaternionEKFParameters::use_magnetometer_measurements
+     * flag to enable the use of magnetometer measurement for yaw correction, default value: false
+     */
+    struct AttitudeQuaternionEKFParameters {
+    double time_step_in_seconds{0.01};
+    double bias_correlation_time_factor{0.01};
+
+    // measurement noise - zero mean, and a given variance
+    double accelerometer_noise_variance{0.001};
+    double magnetometer_noise_variance{0.001};
+
+    // process noise - zero mean, and a given covariance
+    double gyroscope_noise_variance{0.001};
+    double gyro_bias_noise_variance{0.0001};
+
+    double initial_orientation_error_variance{10};
+    double initial_ang_vel_error_variance{10};
+    double initial_gyro_bias_error_variance{10};
+
+    bool use_magnetometer_measurements{false};
+    };
+
+    /**
+     * @class AttitudeQuaternionEKF implements a Quaternion based Discrete Extended Kalman Filter fusing IMU measurements,
+     * to give estimates of orientation, angular velocity and gyroscope bias
+     *
+     * It follows the implementation detailed in
+     * <a href="https://wuecampus2.uni-wuerzburg.de/moodle/pluginfile.php/1109745/mod_resource/content/1/QEKF_Floatsat_WS16.pdf">Quaternion Based Extended Kalman Filter, slides by Michael Stohmeier</a>
+     * The filter is used to estimate the states \f$ X = \begin{bmatrix} {^A}q_B \\ {^B}\Omega_{A,B} \\ {^B}b \end{bmatrix}^T \f$
+     * where \f$ {^A}q_B \in \mathbb{R}^4 \f$  is the quaternion representing the orientation of a body(IMU) frame with respect to an inertial frame ,
+     *       \f$ {^B}\Omega_{A,B} \in \mathbb{R}^3 \f$  is the angular velocity of a body(IMU) frame with respect to an inertial frame, expressed in the body frame and
+     *       \f$ {^B}b \in \mathbb{R}^3 \f$  is the gyroscope bias expressed in the body frame.
+     * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience
+     *
+     * Discretized dynamics during the prediction step,
+     * \f[ \hat{{x}}_{k+1} = \begin{bmatrix} q_k + \Delta t. \frac{1}{2}q_k  \circ \begin{bmatrix} 0 \\ \omega^b_{k+1} \end{bmatrix} \\ y_{gyro_{k}} - b_{k} \\ (1 - \lambda_{b} \Delta t)b_k \end{bmatrix} \f]
+     *
+     * Measurement model for accelerometer is given as,
+     * \f[ h_{acc}(\hat{x}_{k+1}) = \begin{bmatrix} 2(q_1q_3 - q_0q_2) \\ 2(q_2q_3 - q_0q_1) \\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix} \f]
+     * obtained from \f$ {^w}R_b^T e_3 \f$ of the assumed gravity direction.
+     *
+     * Measurement model for magnetometer measurement is given as,
+     * \f[ h_{mag}(\hat{x}_{k+1}) = atan2( 2(q_0q_3 + q_1q_2),1 - 2(q_2^2 + q_3^2) ) \f]
+     *
+     * The linearized system propogation and measurement model is obtained by computing Jacobins F and H with respect to the state.
+     *
+     * The zero mean, additive Gaussian noise can be set using the covariance matrices which will be used during predict and update steps.
+     *
+     * The propagateStates() method is called to set the input vector for the EKF, then ekfPredict() is called to propagate the state through the propagation function f()
+     * and propate the state covariance using the Jacobian F.
+     *
+     * The updateFilterWithMeasurements() is called to set the measurement vector for the EKF, and then ekfUpdate is used to correct the state estimate and its covariance
+     * using the measurement model function h() and the measurement Jacobian H.
+     *
+     * The usage of the QEKF should follow the decribed procedure below,
+     * - instantiate the filter
+     * - set parameters
+     * - call initializeFilter() (this is necessary for resizing the buffers, the user should call this method after setting parameters)
+     * - use setInternalState() to set initial state (The filter will throw an error, if this is not called atleast once, this enforces the user to set intial state)
+     * - Once initialized, the following filter methods can be run in a loop to get the orientation estimates,
+     *     - propagateStates() method to propagate the states and covariance
+     *     - updateFilterWithMeasurements() method to correct the predicted states and covariance
+     *     - getInternalState() or getOrientationEstimate*() methods to get the entire state estimate or only the attitude estimated in desired representation
+     *
+     * @warning calling the method useMagnetometerMeasurements() while the estimator is running, will reset the filter, reinitialize the filter to resize buffers
+     * and sets the previous estiamted state as the inital state.
+     * @note calling other set parameter methods does not reset the filter, since they are not associated with changing the output dimensions
+     *
+     */
+    class AttitudeQuaternionEKF : public IAttitudeEstimator,
+                                  public DiscreteExtendedKalmanFilterHelper
+    {
+    public:
+        AttitudeQuaternionEKF();
+
+        /**
+         * @brief Get filter parameters as a struct.
+         * @param[out] params object of AttitudeQuaternionEKFParameters passed as reference
+         */
+        void getParameters(AttitudeQuaternionEKFParameters& params) {params = m_params;}
+
+        /**
+          * @brief Set filter parameters with the struct members.
+          * This resets filter since it also calls useMagnetometerMeasurements().
+          * @param[in] params object of AttitudeQuaternionEKFParameters passed as a const reference
+          * @return true/false if successful/not
+          */
+        void setParameters(const AttitudeQuaternionEKFParameters& params)
+        {
+            m_params = params;
+            useMagnetometerMeasurements(params.use_magnetometer_measurements);
+        }
+
+        /**
+         * @brief Set the gravity direction assumed by the filter
+         * This affects the measurement model function h() and Jacobian H
+         * @param[in] gravity_dir gravity direction
+         */
+        void setGravityDirection(const iDynTree::Direction& gravity_dir);
+
+        /**
+         * @brief set discretization time step in seconds
+         * @param[in] time_step_in_seconds time step
+         */
+        void setTimeStepInSeconds(double time_step_in_seconds) {m_params.time_step_in_seconds = time_step_in_seconds; }
+
+        /**
+         * @brief set bias correlation time factor
+         * @param[in] bias_correlation_time_factor time factor for bias evolution
+         */
+        void setBiasCorrelationTimeFactor(double bias_correlation_time_factor) { m_params.bias_correlation_time_factor = bias_correlation_time_factor; }
+
+        /**
+         * @brief set flag to use magnetometer measurements
+         * @param[in] use_magnetometer_measurements enable/disable magnetometer measurements
+         * @note calling this method will reset the filter, reinitialize the filter and set the previous state as filter's initial state
+         */
+        bool useMagnetometerMeasurements(bool use_magnetometer_measurements);
+
+        /**
+         * @brief prepares the measurement noise covariance matrix and calls ekfSetMeasurementNoiseMeanAndCovariance()
+         * measurement noise depends only on accelerometer xyz (and magnetometer z)
+         * @note the noise has zero mean (basically passes a zero vector with covariance matrix)
+         * @param[in] acc variance for accelerometer measurements
+         * @param[in] mag variance for magnetometer measurements
+         * @return true/false if successful/not
+         */
+        bool setMeasurementNoiseVariance(double acc, double mag);
+
+        /**
+         * @brief prepares the system noise covariance matrix and calls ekfSetSystemNoiseMeanAndCovariance()
+         * process noise depends on gyro measurement and gyro bias estimate - since gyro measurement is passed as input
+         * @note the noise has zero mean (basically passes a zero vector with covariance matrix)
+         * measurement noise depends only on accelerometer xyz (and magnetometer z)
+         * @param[in] gyro variance for gyroscope measurements
+         * @param[in] gyro_bias variance for gyroscope bias estimates
+         * @return true/false if successful/not
+         */
+        bool setSystemNoiseVariance(double gyro, double gyro_bias);
+
+        /**
+         * @brief prepares the state covariance matrix and calls ekfSetStateCovariance()
+         * @param[in] orientation_var variance for intial orientation state estimate
+         * @param[in] ang_vel_var variance for initial angular velocity state estimate
+         * @param[in] gyro_bias_var variance for intial gyro bias state estimate
+         * @return true/false if successful/not
+         */
+        bool setInitialStateCovariance(double orientation_var, double ang_vel_var, double gyro_bias_var);
+
+        /**
+         * @brief intializes the filter by resizing buffers and setting parameters
+         * - sets state, output and input dimensions for the ekf
+         * - resizes internal buffers
+         * - calls ekfInit()
+         * - sets system noise, measurement noise and initial state covariance
+         * - if successful sets initialized flag to true
+         * @return true/false if successful/not
+         */
+        bool initializeFilter();
+
+
+        bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
+                                          const iDynTree::GyroscopeMeasurements& gyroMeas) override;
+        bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
+                                          const iDynTree::GyroscopeMeasurements& gyroMeas,
+                                          const iDynTree::MagnetometerMeasurements& magMeas) override;
+        bool propagateStates() override;
+        bool getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot) override;
+        bool getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q) override;
+        bool getOrientationEstimateAsRPY(iDynTree::RPY& rpy) override;
+        size_t getInternalStateSize() const override;
+        bool getInternalState(const iDynTree::Span<double> & stateBuffer) const override;
+        bool getDefaultInternalInitialState(const iDynTree::Span<double> & stateBuffer) const override;
+        bool setInternalState(const iDynTree::Span<double> & stateBuffer) override;
+        bool setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) override;
+
+    private:
+        /**
+         * discrete system propagation \f$ f(X, u) = f(X, y_gyro) \f$
+         * where \f$ X = \begin{bmatrix} q_0 &  q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z & \b_x & \b_y & \b_z \end{bmatrix}^T \f$
+         * \f$ u = \begin{bmatrix} y_{gyro}_x & y_{gyro}_y & y_{gyro}_z \end{bmatrix}^T \f$
+         * \f$ f(X, u) = \begin{bmatrix} q + \Delta t. \frac{1}{2}q \circ \begin{bmatrix} 0 \\ \omega^b \end{bmatrix} \\ y_{gyro} - b \\ (1 - \lambda_{b} \Delta t)b \end{bmatrix}\f$
+         */
+        bool ekf_f(const iDynTree::VectorDynSize& x_k,
+               const iDynTree::VectorDynSize& u_k,
+               iDynTree::VectorDynSize& xhat_k_plus_one) override;
+
+        /**
+         * discrete measurement prediction
+         * where \f$ h(X) = \begin{bmatrix} h_{acc}(X) & h_{mag}(X) \end{bmatrix}^T \f$
+         * \f$ h_{acc}(X) = R^T \begin{bmatrix} 0 \\  0 \\ -1 \end{bmatrix} \f$
+         * \f$ h_{mag}(X) = atan2(tan(yaw))\f$
+         */
+        bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one,
+               iDynTree::VectorDynSize& zhat_k_plus_one) override;
+
+        /**
+         * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance
+         *        The analytical Jacobian describing the partial derivative of the system propagation with respect to the state
+         * @param[in] x system state
+         * @param[out] F system Jacobian
+         * @return bool true/false if successful or not
+         */
+        bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) override;
+
+        /**
+         * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance
+         *        The analytical Jacobian describing the partial derivative of the measurement model with respect to the state
+         * @param[in] x system state
+         * @param[out] H measurement Jacobian
+         * @return bool true/false if successful or not
+         */
+        bool ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) override;
+
+        /** @brief prepares the system noise covariance matrix using internal struct params
+         * system  model is as good as gyroscope measurement and bias estimate
+         * system noise covariance can be descibed as \f$ Q = F_u  U  {F_u}^T \f$
+         * where \f$ F = \begin{bmatrix} \frac{\partial f}{\partial y_gyro} & \frac{\partial f}{ \partial x_gyrobias} \end{bmatrix} \f$
+         * \f$ = \begin{bmatrix} 0_{4 \times 3} & 0_{4 \times 3} \\ I_{3 \times 3} & 0_{3 \times 3} \\ 0_{3 \times 3} & I_{3 \times 3} \end{bmatrix}\f$
+         * \f$ U = diag(\begin{bmatrix} \sigma_{gyro}^{2} I_{3 \times 3} & \sigma_{gyrobias}^{2} I_{3 \times 3} \end{bmatrix}) \f$
+         * @param[in] Q matrix container as reference
+         */
+        void prepareSystemNoiseCovarianceMatrix(iDynTree::MatrixDynSize &Q);
+
+        /** @brief prepares the measurement noise covariance matrix using internal struct parameters
+         * measurement noise depends only on accelerometer measurement along x-,y- and z- directions
+         * along with magnetometer z-direction if included
+         * measurement noise covariance can be descibed as,
+         * \f$ R = \begin{bmatrix} \sigma_{acc}^{2} I_{3 \times 3} & 0_{3 \times 1} \\ 0_{1 \times 3} & \sigma_{mag}^{2}\f$
+         * if magnetometer measurements is also considered. In case of magnetometer measurements not being considered, it is reduced
+         * to the \f$ 3 \times 3 \f$ matrix
+         * @param[in] R matrix container as reference
+         */
+        void prepareMeasurementNoiseCovarianceMatrix(iDynTree::MatrixDynSize &R);
+
+        /**
+         * @brief serializes the state struct to state x of VectorDynSize
+         */
+        void serializeStateVector();
+
+        /**
+         * @brief deserializes state x of VectorDynSize to the state struct
+         */
+        void deserializeStateVector();
+
+        /**
+         * @brief serializes the accelerometer and magenetometer measurements into y vector
+         * since DiscreteExtendedKalmanFilter expects a VectorDynSize including all necessary measurements
+         */
+        void serializeMeasurementVector();
+
+
+        /**
+         * @brief serializes measurements, calls ekfUpdate step and
+         * gets state estimate corrected by measurements
+         *
+         * @return true/false, if successful/not
+         */
+        bool callEkfUpdate();
+
+        AttitudeQuaternionEKFParameters m_params;   ///< struct holding the QEKF parameters
+
+        /** @struct state internal state of the estimator
+         * @var state::m_orientation
+         * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation
+         * @var state::m_orientation
+         * angular velocity estimate in \f$ \mathbb{R}^3 \f$
+         * @var state::m_orientation
+         * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$
+         */
+        struct {
+            iDynTree::UnitQuaternion m_orientation;
+            iDynTree::Vector3 m_angular_velocity;
+            iDynTree::Vector3 m_gyroscope_bias;
+        } m_state, m_initial_state;
+
+        iDynTree::Rotation m_orientationInSO3;                   ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body
+        iDynTree::RPY m_orientationInRPY;                        ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$
+
+        iDynTree::GyroscopeMeasurements m_Omega_y;               ///< 3d gyroscope measurement giving angular velocity of body wrt inertial frame, expressed in body frame
+        iDynTree::LinearAccelerometerMeasurements m_Acc_y;       ///< 3d accelerometer measurement giving proper classical acceleration expressed in body frame
+        double m_Mag_y;                                          ///< magnetometer yaw measurement expressed in body frame
+
+        iDynTree::VectorDynSize m_x;                             ///< state vector for the EKF - orientation, angular velocity, gyro bias
+        iDynTree::VectorDynSize m_y;                             ///< measurement vector for the EKF - accelerometer (and magnetometer yaw)
+        iDynTree::VectorDynSize m_u;                             ///< input vector for the EKF - gyroscope measurement
+
+        size_t m_state_size;                                     ///< state dimensions
+        size_t m_output_size;                                    ///< output dimensions
+        size_t m_input_size;                                     ///< input dimensions
+        bool m_initialized{false};                               ///< flag to check if QEKF is initialized
+
+        iDynTree::Matrix4x4 m_Id4;                               ///< \f$ 4 \times 4 \f$  identity matrix
+        iDynTree::Matrix3x3 m_Id3;                               ///< \f$ 3 \times 3 \f$  identity matrix
+        iDynTree::Direction m_gravity_direction;                 ///< direction of the gravity vector expressed in the inertial frame denoted by \f$ A \f$, default set to \f$ e_3 = \begin{bmatrix} 0 & 0 & 1.0 \end{bmatrix}^T \f$
+    };
+
+}
+
+#endif
diff --git a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
new file mode 100644
index 00000000000..a6ea5e166cb
--- /dev/null
+++ b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
@@ -0,0 +1,282 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef EXTENDED_KALMAN_FILTER_H
+#define EXTENDED_KALMAN_FILTER_H
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/EigenHelpers.h>
+#include <vector>
+
+namespace iDynTree
+{
+
+    /**
+     * @class DiscreteExtendedKalmanFilterHelper naive base class implementation of discrete EKF with additive Gaussian noise
+     *
+     * @warning This class is not stand-alone but can be used meaningfully only by classes deriving from it.
+     * The system propagation function f() and measurement model function h() are virtual functions to be defined by
+     * the derived class implementing the EKF.
+     * Similarly, the Jacobians for the linearized system propagation and measurement model are also virtual functions
+     * to be implemented by the derived class.
+     *
+     * The derived class must must set the size for states, inputs and outputs using ekfSetStateSize(), ekfSetInputSize() and ekfSetOutputSize() methods.
+     * Then the derived class must call ekfInit() to resize the vectors and matrices for the EKF.
+     *
+     * Similarly before running the estimator through a loop, it is recommended to set the initial states and variances for measurements, system dynamics and intial states.
+     * This is necessary to avoid any NaN values to be propagated.
+     *
+     * Once, initialized properly, the filter can be run by
+     *  - calling ekfSetInputVector() to set the control inputs and then calling ekfPredict() at each prediction step, and
+     *  - calling ekfSetMeasurementVector() to set the measurements and then calling ekfUpdate() at each update step
+     *
+     * The internal state of the estimator can be obtained by calling ekfGetStates().
+     *
+     * The Discrete Extended Kalman Filter equations implemented in this class are coherent with the ones described in
+     * <a href="https://en.wikipedia.org/wiki/Extended_Kalman_filter">Discrete-time predict and update equations section of this article.</a>
+     *
+     * The general workflow implementing/inheriting this class would be in the order,
+     * - call ekfSetInputSize(), ekfSetOutputSize(), ekfSetStateSize() methods
+     * - call ekfInit()
+     * - call ekfSetInitialState(), ekfSetStateCovariance() (either done externally later or internally. usually done externally later, however, filter runs properly only if this step is done)
+     * - call ekfSetSystemNoiseCovariance(), ekfSetMeasurementNoiseCovariance()
+     *
+     *   once this is setup,
+     * - in a loop
+     *     - call ekfSetInputVector() then ekfPredict(). Calling ekfGetStates() and ekfGetStateCovariance() at this point will give us the predicted states and its covariance
+     *     - call ekfSetMeasurementVector() then ekfUpdate(). Calling ekfGetStates() and ekfGetStateCovariance() at this point will give us the updated states and its covariance
+     * @note if we intend to change the state size, input size or output size on the fly, it is crucial to call the ekfInit() method again, since this resizes the buffers accordingly
+     *       failing to do so will result in memory leaks and will cause the program to crash.
+     *
+     */
+    class DiscreteExtendedKalmanFilterHelper
+    {
+    protected:
+        DiscreteExtendedKalmanFilterHelper();
+
+        /**
+         * @brief Describes the state propagation for a given dynamical system
+         *        If state of the system is denoted by \f$ x \f$ and the control input by \f$ u \f$, then
+         *        the system dynamics is given as \f$ x_{k+1} = f(x_k, u_k) \f$.
+         * @note the detail of this function needs to be implemented by the child class
+         * @param[in] x_k state at current time instant
+         * @param[in] u_k control input at current time instant
+         * @param[out] xhat_k_plus_one predicted state without any correction from measurements
+         * @return bool true/false if successful or not
+         */
+        virtual bool ekf_f(const iDynTree::VectorDynSize& x_k,
+                       const iDynTree::VectorDynSize& u_k,
+                             iDynTree::VectorDynSize& xhat_k_plus_one) = 0;
+
+        /**
+         * @brief Describes the measurement model of the system,
+         *        i.e., how the measurements can be described as a function of states,
+         *        Given a state of the system described by \f$ x \f$,
+         *        what would be the measurement \f$ z \f$ observed from this state \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$.
+         * @note the detail of this function needs to be implemented by the child class
+         * @param[in] xhat_k_plus_one predicted state of next time instant
+         * @param[out] zhat_k_plus_one predicted measurement of next time instant
+         * @return bool true/false if successful or not
+         */
+        virtual bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one,
+                             iDynTree::VectorDynSize& zhat_k_plus_one) = 0;
+
+        /**
+         * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance
+         *        The analytical Jacobian describing the partial derivative of the system propagation with respect to the state
+         * @note the detail of this function needs to be implemented by the child class
+         * @param[in] x system state
+         * @param[out] F system Jacobian
+         * @return bool true/false if successful or not
+         */
+        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0;
+
+        /**
+         * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance
+         *        The analytical Jacobian describing the partial derivative of the measurement model with respect to the state
+         * @note the detail of this function needs to be implemented by the child class
+         * @param[in] x system state
+         * @param[out] H measurement Jacobian
+         * @return bool true/false if successful or not
+         */
+        virtual bool ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H) = 0;
+
+        /**
+         * @brief Implements the Discrete EKF prediction equation
+         *        described by
+         *        \f$ \hat{x}_{k+1} = f(x_k, u_k) \f$ is given by the ekf_f() method
+         *        \f$ \hat{P}_{k+1} = F_k P_k F_k^T + Q \f$
+         *        where, \f$ F \mid_{x = x_k} \f$ is given by the ekfComputejacobianF() method
+         *
+         * @warning this function can be called only after setting up the filter properly through ekfInit() step
+         * @note this function should be once called every step, after setting up the input vector using ekfSetInputVector() method
+         * @warning setting up the input vector everytime before calling the ekfPredict() method is crucial, the prediction step is not performed if this step is skipped
+         * this is because internally a flag associated to the setting up of input vector is set true by ekfSetInputVector() method which in turn is set false by ekfPredict() method
+         * @return bool true/false if successful or not
+         */
+        bool ekfPredict();
+
+        /**
+         * @brief Implements the Discrete EKF update equation
+         *        described by
+         *        \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$ is given by ekf_h() method
+         *        innovation \f$ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} \f$
+         *        innovation covariance \f$ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R \f$, where \f$ H \mid_{x = \hat{x}_{k+1}} \f$ is given by ekfComputejacobianH() method
+         *        Kalman gain \f$ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} \f$
+         *        Updated covariance \f$ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) \f$
+         *        Updated state estimate \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} \f$
+         *
+         * @warning this function can be called only after setting up the filter properly through ekfInit() step
+         * @note this function should be once called every step, after setting up the measurement vector using ekfSetMeasurementVector() method
+         * @warning setting up the measurement vector everytime before calling the ekfUpdate() method is crucial, the update step is not performed if this step is skipped
+         * this is because internally a flag associated to the setting up of measurement vector is set true by ekfSetMeasurementVector() method which in turn is set false by ekfUpdate() method
+         * @return bool true/false if successful or not
+         */
+        bool ekfUpdate();
+
+        /**
+         * @brief Initializes and resizes the internal buffers of this filter
+         * @warning this is a very crucial method of this class. This needs to be called after setting the input size through ekfSetInputSize(),
+         *          output size through ekfSetOutputSize() and state dimension through ekfSetStateSize(),
+         *          such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions.
+         *          Failing to do so might result in memory leaks and may cause the program to crash
+         * @return bool true/false if successful or not
+         */
+        bool ekfInit();
+
+        /**
+         * @brief Resets the filter flags
+         * The filter flags check if the filter was properly initialized, if the initial state was set,
+         * if the initial state covariance was set. These three flags are crucial for proper setting up of the filter.
+         * The other flags include the checks on whether the input and measurement vectors were updated at every prediction/update step
+         */
+        void ekfReset();
+
+        /**
+         * @brief Set measurement vector at every time step
+         * the measurement vector size and output size should match
+         * @param[in] y iDynTree::Span object to access the measurement vector
+         * @return bool true/false if successful or not
+         */
+        bool ekfSetMeasurementVector(const iDynTree::Span<double>& y);
+
+        /**
+         * @brief Set input vector at every time step
+         * the input vector size and input size should match
+         * @param[in] u iDynTree::Span object to access the input vector
+         * @return bool true/false if successful or not
+         */
+        bool ekfSetInputVector(const iDynTree::Span<double>& u);
+
+        /**
+         * @brief Set initial state
+         * the size of x0 and state size should match
+         * @param[in] x0 iDynTree::Span object to access the state vector
+         * @note this method should be called before running the filter
+         * @return bool true/false if successful or not
+         */
+        bool ekfSetInitialState(const iDynTree::Span<double>& x0);
+
+        /**
+         * @brief Set initial state covariance matrix
+         * the size of P and (state size*state size) should match
+         * @param[in] P iDynTree::Span object to access the state covariance matrix
+         * @note this method should be called before running the filter
+         * @warning if this matrix is not initialized properly, then the resulting output will only have NaNs in it
+         * @return bool true/false if successful or not
+         */
+        bool ekfSetStateCovariance(const iDynTree::Span<double>& P);
+
+        /**
+         * @brief Set system noise covariance matrix
+         * the size of Q and (state size*state size) should match
+         * @param[in] Q iDynTree::Span object to access the system noise covariance matrix
+         * @note default value is a zero matrix
+         * @return bool true/false if successful or not
+         */
+        bool ekfSetSystemNoiseCovariance(const iDynTree::Span<double>& Q);
+
+        /**
+         * @brief Set measurement noise covariance matrix
+         * the size of R and (output size*output size) should match
+         * @param[in] R iDynTree::Span object to access the measurement noise covariance matrix
+         * @note default value is a zero matrix
+         * @return bool true/false if successful or not
+         */
+        bool ekfSetMeasurementNoiseCovariance(const iDynTree::Span<double>& R);
+
+        /**
+         * @brief Set the state dimensions
+         * @param[in] dim_X state size
+         * @warning this method should be called before calling ekfInit()
+         * @return bool true/false if successful or not
+         */
+        void ekfSetStateSize(size_t dim_X) { m_dim_X = dim_X; }
+
+        /**
+         * @brief Set the input dimensions
+         * @param[in] dim_U input size
+         * @warning this method should be called before calling ekfInit()
+         * @return bool true/false if successful or not
+         */
+        void ekfSetInputSize(size_t dim_U) { m_dim_U = dim_U; }
+
+        /**
+         * @brief Set the ouptut dimensions
+         * @param[in] dim_Y output size
+         * @warning this method should be called before calling ekfInit()
+         * @return bool true/false if successful or not
+         */
+        void ekfSetOutputSize(size_t dim_Y) { m_dim_Y = dim_Y; }
+
+        /**
+         * @brief Get current internal state of the filter
+         * the size of x and state size should match
+         * @param[out] x iDynTree::Span object to copy the internal state vector into
+         * @return bool true/false if successful or not
+         */
+        bool ekfGetStates(const iDynTree::Span<double> &x) const;
+
+        /**
+         * @brief Get state covariance matrix
+         * the size of P and (state size*state size) should match
+         * @param[out] P iDynTree::Span object to copy the internal state covariance matrix onto
+         * @return bool true/false if successful or not
+         */
+        bool ekfGetStateCovariance(const iDynTree::Span<double> &P) const;
+
+    private:
+        size_t m_dim_X;                                ///< state dimension
+        size_t m_dim_Y;                                ///< output dimenstion
+        size_t m_dim_U;                                ///< input dimension
+        iDynTree::VectorDynSize m_x;                   ///< state at time instant k
+        iDynTree::VectorDynSize m_u;                   ///< input at time instant k
+        iDynTree::VectorDynSize m_y;                   ///< measurements at time instant k
+        iDynTree::VectorDynSize m_xhat;                ///< predicted state at time instant k before updating measurements
+
+        iDynTree::MatrixDynSize m_F;                   ///< System jacobian
+        iDynTree::MatrixDynSize m_P;                   ///< State covariance
+        iDynTree::MatrixDynSize m_Phat;                ///< State covariance estimate before updating measurements
+        iDynTree::MatrixDynSize m_Q;                   ///< system noise covariance
+        iDynTree::MatrixDynSize m_H;                   ///< measurement jacobian
+        iDynTree::MatrixDynSize m_S;                   ///< innovation covariance
+        iDynTree::MatrixDynSize m_K;                   ///< Kalman gain
+        iDynTree::MatrixDynSize m_R;                   ///< measurement noise covariance
+        bool m_is_initialized{false};                  ///< flag to check if filter is properly initialized
+        bool m_measurement_updated{false};             ///< flag to check if measurement is updated at each update step
+        bool m_input_updated{false};                   ///< flag to check if control input is updated at each prediction step
+        bool m_initial_state_set{false};               ///< flag to check if the initial state of the filter is set
+        bool m_initial_state_covariance_set{false};    ///< flag to check if the initial covariance is set properly
+    };
+}
+
+#endif
diff --git a/src/estimation/src/AttitudeEstimator.cpp b/src/estimation/src/AttitudeEstimator.cpp
new file mode 100644
index 00000000000..6b14216c16c
--- /dev/null
+++ b/src/estimation/src/AttitudeEstimator.cpp
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 : 
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/AttitudeEstimator.h>
+
+
+iDynTree::IAttitudeEstimator::~IAttitudeEstimator()
+{
+}
diff --git a/src/estimation/src/AttitudeEstimatorUtils.cpp b/src/estimation/src/AttitudeEstimatorUtils.cpp
new file mode 100644
index 00000000000..6aaa50b177f
--- /dev/null
+++ b/src/estimation/src/AttitudeEstimatorUtils.cpp
@@ -0,0 +1,138 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
+#include <vector>
+
+iDynTree::Vector3 crossVector(const iDynTree::Vector3& a, const iDynTree::Vector3& b)
+{
+    using iDynTree::toEigen;
+
+    iDynTree::Vector3 out;
+    toEigen(out) = toEigen(a).cross(toEigen(b)); // to be read as out = a.cross(b)
+
+    return out;
+}
+
+iDynTree::Matrix3x3 mapR3Toso3(const iDynTree::Vector3& omega)
+{
+    using iDynTree::toEigen;
+
+    iDynTree::Matrix3x3 out;
+    toEigen(out) = iDynTree::skew(toEigen(omega)); // to be read as out = skew(omega)
+
+    return out;
+}
+
+bool checkSkewSymmetricity(const iDynTree::Matrix3x3& S)
+{
+    using iDynTree::toEigen;
+
+    bool flag = false;
+    if ( (toEigen(S) + toEigen(S).transpose()).isZero() ) // to be read as (S + S.transpose()).isZero()
+    {
+        flag = true;
+    }
+
+    return flag;
+}
+
+iDynTree::Vector3 mapso3ToR3(const iDynTree::Matrix3x3& S)
+{
+    using iDynTree::toEigen;
+
+    iDynTree::Vector3 out;
+    toEigen(out) = iDynTree::unskew(toEigen(S));  // to be read as out = unskew(S)
+
+    return out;
+}
+
+double innerProduct(const iDynTree::Vector3 a, const iDynTree::Vector3& b)
+{
+    using iDynTree::toEigen;
+
+    return toEigen(a).dot(toEigen(b)); // to be read as a.dot(b)
+}
+
+double realPartOfQuaternion(const iDynTree::UnitQuaternion& q)
+{
+    return q(0);
+}
+
+iDynTree::Vector3 imaginaryPartOfQuaternion(const iDynTree::UnitQuaternion &q)
+{
+    iDynTree::Vector3 v;
+    v(0) = q(1);
+    v(1) = q(2);
+    v(2) = q(3);
+    return v;
+}
+
+iDynTree::UnitQuaternion composeQuaternion(const iDynTree::UnitQuaternion &q1, const iDynTree::UnitQuaternion &q2)
+{
+    using iDynTree::toEigen;
+
+    double s1 = realPartOfQuaternion(q1);
+    iDynTree::Vector3 v1 = imaginaryPartOfQuaternion(q1);
+
+    double s2 = realPartOfQuaternion(q2);
+    iDynTree::Vector3 v2 = imaginaryPartOfQuaternion(q2);
+
+    iDynTree::Vector3 imagOut;
+    toEigen(imagOut) = toEigen(v2)*s1 + toEigen(v1)*s2 + toEigen(crossVector(v1, v2)); // to be read as imagOut = v2*s1 + v1*s2 + crossVector(v1, v2)
+
+    iDynTree::UnitQuaternion out;
+    out(0) = s1*s2 - innerProduct(v1, v2);
+    out(1) = imagOut(0);
+    out(2) = imagOut(1);
+    out(3) = imagOut(2);
+    toEigen(out).normalize();
+
+    return out;
+}
+
+iDynTree::Matrix4x4 mapofYQuaternionToXYQuaternion(const iDynTree::UnitQuaternion &x)
+{
+    // unitary matrix structure represented by 2 complex numbers z1 = q0+iq1 and z2 = q2+iq3
+    std::vector<double> v{x(0), -x(1), -x(2), -x(3),
+                          x(1),  x(0), -x(3),  x(2),
+                          x(2),  x(3),  x(0), -x(1),
+                          x(3), -x(2),  x(1),  x(0)};
+    return iDynTree::Matrix4x4(v.data(), 4, 4);
+}
+
+iDynTree::UnitQuaternion composeQuaternion2(const iDynTree::UnitQuaternion &q1, const iDynTree::UnitQuaternion &q2)
+{
+    // this function is computationally faster than composeQuaternion
+    using iDynTree::toEigen;
+
+    iDynTree::UnitQuaternion out;
+    toEigen(out) = toEigen(mapofYQuaternionToXYQuaternion(q1))*toEigen(q2); // to be read as out = mapofYQuaternionToXYQuaternion(q1)*q2
+    toEigen(out).normalize();
+
+    return out;
+}
+
+iDynTree::UnitQuaternion pureQuaternion(const iDynTree::Vector3& bodyFixedFrameVelocityInInertialFrame)
+{
+    iDynTree::UnitQuaternion p;
+    p(0) = 0;
+    p(1) = bodyFixedFrameVelocityInInertialFrame(0);
+    p(2) = bodyFixedFrameVelocityInInertialFrame(1);
+    p(3) = bodyFixedFrameVelocityInInertialFrame(2);
+    return p;
+}
+
+// method definition copied from The art of programming by Knuth
+bool checkDoublesApproximatelyEqual(double val1, double val2, double tol)
+{
+    return fabs(val1 - val2) <= ( (fabs(val1) < fabs(val2) ? fabs(val2) : fabs(val1)) * tol );
+}
+
diff --git a/src/estimation/src/AttitudeMahonyFilter.cpp b/src/estimation/src/AttitudeMahonyFilter.cpp
new file mode 100644
index 00000000000..4d08dc93d83
--- /dev/null
+++ b/src/estimation/src/AttitudeMahonyFilter.cpp
@@ -0,0 +1,263 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/AttitudeMahonyFilter.h>
+#include <ctime>
+
+iDynTree::Matrix3x3 getMatrixFromVectorVectorMultiplication(iDynTree::Vector3 a, iDynTree::Vector3 b)
+{
+    using iDynTree::toEigen;
+    iDynTree::Matrix3x3 out;
+
+    toEigen(out) = toEigen(a)*toEigen(b).transpose(); // to be read as out = a*(b.transpose())
+
+    return out;
+}
+
+iDynTree::Matrix3x3 getAngVelSkewSymmetricMatrixFromMeasurements(iDynTree::Vector3 meas, const iDynTree::Direction& vectorDir, double confidenceMeas, const iDynTree::Rotation& R)
+{
+    using iDynTree::toEigen;
+
+    ///< compute \f$ {{^A}{\hat{R}}_B}^T e_3 \f$ where \f$ e_3 \f$ is the vectorDir
+    iDynTree::Direction va_hat = R.inverse().changeCoordFrameOf(vectorDir);
+    iDynTree::Vector3 vectorial_estimate = iDynTree::Vector3(va_hat.data(), 3);
+
+    ///< compute vectorial direction from the measurement normalized
+    toEigen(meas).normalize();
+
+    iDynTree::Matrix3x3 Adyn = getMatrixFromVectorVectorMultiplication(meas, vectorial_estimate);
+    iDynTree::Matrix3x3 Sdyn;
+    auto A(toEigen(Adyn));
+    auto S(toEigen(Sdyn));
+
+    S = (A - A.transpose())*(confidenceMeas*0.5);
+
+    return Sdyn;
+}
+
+bool iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas)
+{
+    using iDynTree::toEigen;
+
+    iDynTree::Matrix3x3 S_acc = getAngVelSkewSymmetricMatrixFromMeasurements(linAccMeas, m_gravity_direction, 1-m_params.confidence_magnetometer_measurements, m_orientationInSO3);
+    toEigen(m_omega_mes) = -toEigen(mapso3ToR3(S_acc));
+    m_Omega_y = gyroMeas;
+
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas, const iDynTree::MagnetometerMeasurements& magMeas)
+{
+    using iDynTree::toEigen;
+
+    if (m_params.use_magnetometer_measurements == false)
+    {
+        iDynTree::reportWarning("AttitudeMahonyFilter", "updateFilterWithMeasurements", "useMagnetoMeterMeasurements set to false, using only accelerometer measurements");
+        return updateFilterWithMeasurements(linAccMeas, gyroMeas);
+    }
+
+    iDynTree::Matrix3x3 S_acc = getAngVelSkewSymmetricMatrixFromMeasurements(linAccMeas, m_gravity_direction, 1-m_params.confidence_magnetometer_measurements, m_orientationInSO3);
+    iDynTree::Matrix3x3 S_mag = getAngVelSkewSymmetricMatrixFromMeasurements(magMeas, m_earth_magnetic_field_direction, m_params.confidence_magnetometer_measurements, m_orientationInSO3);
+    iDynTree::Matrix3x3 S_meas;
+
+    toEigen(S_meas) = toEigen(S_acc) + toEigen(S_mag);
+    toEigen(m_omega_mes) = -toEigen(mapso3ToR3(S_meas));
+    m_Omega_y = gyroMeas;
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::propagateStates()
+{
+    using iDynTree::toEigen;
+    iDynTree::Vector3 gyro_update_dyn;
+    auto q(toEigen(m_state.m_orientation));
+    auto Omega(toEigen(m_state.m_angular_velocity));
+    auto b(toEigen(m_state.m_gyroscope_bias));
+    auto gyroUpdate(toEigen(gyro_update_dyn));
+    auto Omega_y(toEigen(m_Omega_y));
+    auto omega_mes(toEigen(m_omega_mes));
+
+    // compute the correction from the measurements
+    gyroUpdate = Omega_y - b + (omega_mes*m_params.kp);
+    iDynTree::UnitQuaternion correction = pureQuaternion(gyro_update_dyn);
+    auto dq(toEigen(composeQuaternion2(m_state.m_orientation, correction)));
+
+    // system dynamics equations
+    q = q + (dq*(m_params.time_step_in_seconds*0.5));
+    q.normalize();
+    Omega = Omega_y - b;
+    b = b - (omega_mes*m_params.ki)*(m_params.time_step_in_seconds);
+
+    m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation);
+    m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation).asRPY();
+    return true;
+}
+
+iDynTree::AttitudeMahonyFilter::AttitudeMahonyFilter()
+{
+    m_params.time_step_in_seconds = 0.01;
+    m_params.kp = 1.0;
+    m_params.ki = 0.0;
+    m_params.use_magnetometer_measurements = false;
+    m_params.confidence_magnetometer_measurements = 0.0;
+
+    m_state.m_orientation.zero();
+    m_state.m_orientation(0) = 1.0;
+    m_state.m_angular_velocity.zero();
+    m_state.m_gyroscope_bias.zero();
+
+    m_initial_state = m_state;
+
+    m_gravity_direction.zero();
+    m_gravity_direction(2) = 1.0; // pointing downwards
+
+    m_earth_magnetic_field_direction.zero();
+    m_earth_magnetic_field_direction(2) = 1.0;
+
+    m_orientationInSO3.fromQuaternion(m_state.m_orientation);
+    m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation).asRPY();
+
+    m_omega_mes.zero();
+    m_Omega_y.zero();
+}
+
+
+void iDynTree::AttitudeMahonyFilter::setTimeStepInSeconds(double timestepInSeconds)
+{
+    m_params.time_step_in_seconds = timestepInSeconds;
+}
+
+void iDynTree::AttitudeMahonyFilter::setConfidenceForMagnetometerMeasurements(double confidence)
+{
+    if (m_params.use_magnetometer_measurements == false)
+    {
+        iDynTree::reportWarning("AttitudeMahonyFilter", "setConfidenceForMagnetometerMeasurements", "not using magnetometer measurements, setting confidence to zero.");
+        m_params.confidence_magnetometer_measurements = 0.0;
+    }
+    m_params.confidence_magnetometer_measurements = confidence;
+}
+
+void iDynTree::AttitudeMahonyFilter::setGainki(double ki)
+{
+    m_params.ki = ki;
+}
+
+void iDynTree::AttitudeMahonyFilter::setGainkp(double kp)
+{
+    m_params.kp = kp;
+}
+
+void iDynTree::AttitudeMahonyFilter::useMagnetoMeterMeasurements(bool flag)
+{
+    m_params.use_magnetometer_measurements = flag;
+    if (flag == false)
+    {
+        iDynTree::reportWarning("AttitudeMahonyFilter", "useMagnetoMeterMeasurements", "setting confidence on magnetometer measurements to zero.");
+        m_params.confidence_magnetometer_measurements = 0.0;
+    }
+}
+
+void iDynTree::AttitudeMahonyFilter::setGravityDirection(const iDynTree::Direction& gravity_dir)
+{
+    m_gravity_direction = gravity_dir;
+}
+
+
+bool iDynTree::AttitudeMahonyFilter::getDefaultInternalInitialState(const iDynTree::Span< double >& stateBuffer) const
+{
+    stateBuffer(0) = m_initial_state.m_orientation(0);
+    stateBuffer(1) = m_initial_state.m_orientation(1);
+    stateBuffer(2) = m_initial_state.m_orientation(2);
+    stateBuffer(3) = m_initial_state.m_orientation(3);
+    stateBuffer(4) = m_initial_state.m_angular_velocity(0);
+    stateBuffer(5) = m_initial_state.m_angular_velocity(1);
+    stateBuffer(6) = m_initial_state.m_angular_velocity(2);
+    stateBuffer(7) = m_initial_state.m_gyroscope_bias(0);
+    stateBuffer(8) = m_initial_state.m_gyroscope_bias(1);
+    stateBuffer(9) = m_initial_state.m_gyroscope_bias(2);
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::getInternalState(const iDynTree::Span< double >& stateBuffer) const
+{
+    stateBuffer(0) = m_state.m_orientation(0);
+    stateBuffer(1) = m_state.m_orientation(1);
+    stateBuffer(2) = m_state.m_orientation(2);
+    stateBuffer(3) = m_state.m_orientation(3);
+    stateBuffer(4) = m_state.m_angular_velocity(0);
+    stateBuffer(5) = m_state.m_angular_velocity(1);
+    stateBuffer(6) = m_state.m_angular_velocity(2);
+    stateBuffer(7) = m_state.m_gyroscope_bias(0);
+    stateBuffer(8) = m_state.m_gyroscope_bias(1);
+    stateBuffer(9) = m_state.m_gyroscope_bias(2);
+    return true;
+}
+
+size_t iDynTree::AttitudeMahonyFilter::getInternalStateSize() const
+{
+    size_t size = 0;
+    size += m_state.m_orientation.size();
+    size += m_state.m_angular_velocity.size();
+    size += m_state.m_gyroscope_bias.size();
+    return size;
+}
+
+bool iDynTree::AttitudeMahonyFilter::getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q)
+{
+    q = m_state.m_orientation;
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot)
+{
+    rot = m_orientationInSO3;
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::getOrientationEstimateAsRPY(iDynTree::RPY& rpy)
+{
+    rpy = m_orientationInRPY;
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::setInternalState(const iDynTree::Span< double >& stateBuffer)
+{
+    if ((size_t)stateBuffer.size() != getInternalStateSize())
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "setInternalState", "state size mismatch, using default state");
+        return false;
+    }
+    m_state.m_orientation(0) = stateBuffer(0);
+    m_state.m_orientation(1) = stateBuffer(1);
+    m_state.m_orientation(2) = stateBuffer(2);
+    m_state.m_orientation(3) = stateBuffer(3);
+    m_state.m_angular_velocity(0) = stateBuffer(4);
+    m_state.m_angular_velocity(1) = stateBuffer(5);
+    m_state.m_angular_velocity(2) = stateBuffer(6);
+    m_state.m_gyroscope_bias(0) = stateBuffer(7);
+    m_state.m_gyroscope_bias(1) = stateBuffer(8);
+    m_state.m_gyroscope_bias(2) = stateBuffer(9);
+    return true;
+}
+
+bool iDynTree::AttitudeMahonyFilter::setInternalStateInitialOrientation(const iDynTree::Span< double >& orientationBuffer)
+{
+    if ((size_t)orientationBuffer.size() != m_state.m_orientation.size())
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "setInternalStateInitialOrientation", "orientation size mismatch, using default state");
+        return false;
+    }
+    m_state.m_orientation(0) = orientationBuffer(0);
+    m_state.m_orientation(1) = orientationBuffer(1);
+    m_state.m_orientation(2) = orientationBuffer(2);
+    m_state.m_orientation(3) = orientationBuffer(3);
+
+    return true;
+}
diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp
new file mode 100644
index 00000000000..0e466de809a
--- /dev/null
+++ b/src/estimation/src/AttitudeQuaternionEKF.cpp
@@ -0,0 +1,651 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/AttitudeQuaternionEKF.h>
+
+void iDynTree::AttitudeQuaternionEKF::serializeStateVector()
+{
+    using iDynTree::toEigen;
+    auto x(toEigen(m_x));
+    auto q(toEigen(m_state.m_orientation));
+    auto Omega(toEigen(m_state.m_angular_velocity));
+    auto b(toEigen(m_state.m_gyroscope_bias));
+
+    if (m_x.size() != m_state_size)
+    {
+        m_x.resize(m_state_size);
+    }
+
+    x.block<4, 1>(0, 0) = q;
+    x.block<3, 1>(4, 0) = Omega;
+    x.block<3, 1>(7, 0) = b;
+}
+
+void iDynTree::AttitudeQuaternionEKF::deserializeStateVector()
+{
+    using iDynTree::toEigen;
+    auto x(toEigen(m_x));
+    auto q(toEigen(m_state.m_orientation));
+    auto Omega(toEigen(m_state.m_angular_velocity));
+    auto b(toEigen(m_state.m_gyroscope_bias));
+
+    q = x.block<4, 1>(0, 0);
+    Omega = x.block<3, 1>(4, 0);
+    b = x.block<3, 1>(4, 0);
+}
+
+void iDynTree::AttitudeQuaternionEKF::prepareSystemNoiseCovarianceMatrix(iDynTree::MatrixDynSize &Q)
+{
+    using iDynTree::toEigen;
+
+    if (Q.rows() != m_state_size && Q.cols() != m_state_size)
+    {
+        Q.resize(m_state_size, m_state_size);
+    }
+
+
+    iDynTree::MatrixDynSize Fu_dyn(m_state_size, m_input_size + m_state.m_gyroscope_bias.size());
+    Fu_dyn.zero();
+    iDynTree::Matrix6x6 U_dyn;
+    U_dyn.zero();
+
+    auto F_u(toEigen(Fu_dyn));
+    auto U(toEigen(U_dyn));
+    auto Id3(toEigen(m_Id3));
+    auto Q_(toEigen(Q));
+
+    F_u.block<3, 3>(4, 0) = Id3;
+    F_u.block<3,3>(7, 3) = Id3;
+
+    U.block<3, 3>(0, 0) = Id3*m_params.gyroscope_noise_variance;
+    U.block<3, 3>(3, 3) = Id3*m_params.gyro_bias_noise_variance;
+
+    Q_ = F_u*U*F_u.transpose();
+}
+
+void iDynTree::AttitudeQuaternionEKF::prepareMeasurementNoiseCovarianceMatrix(iDynTree::MatrixDynSize& R)
+{
+    using iDynTree::toEigen;
+
+    if (R.rows() != m_output_size && R.cols() != m_output_size)
+    {
+        R.resize(m_output_size, m_output_size);
+        R.zero();
+    }
+
+    auto R_(toEigen(R));
+    auto Id3(toEigen(m_Id3));
+
+    R_.block<3, 3>(0, 0) = Id3*m_params.accelerometer_noise_variance;
+    if (m_params.use_magnetometer_measurements)
+    {
+        R_(3, 3) = m_params.magnetometer_noise_variance;
+    }
+}
+
+iDynTree::AttitudeQuaternionEKF::AttitudeQuaternionEKF()
+{
+    m_state.m_orientation.zero();
+    m_state.m_orientation(0) = 1.0;
+    m_state.m_angular_velocity.zero();
+    m_state.m_gyroscope_bias.zero();
+
+    m_initial_state = m_state;
+    m_orientationInSO3.fromQuaternion(m_state.m_orientation);
+    m_orientationInRPY = m_orientationInSO3.asRPY();
+
+    m_Omega_y.zero();
+    m_Acc_y.zero();
+
+    m_gravity_direction.zero();
+    m_gravity_direction(2) = 1;
+
+    using iDynTree::toEigen;
+    toEigen(m_Id4).setIdentity();
+    toEigen(m_Id3).setIdentity();
+}
+
+bool iDynTree::AttitudeQuaternionEKF::initializeFilter()
+{
+    m_state_size = this->getInternalStateSize();
+
+    if (m_params.use_magnetometer_measurements)
+    {
+        m_output_size = output_dimensions_with_magnetometer;
+    }
+    else
+    {
+        m_output_size = output_dimensions_without_magnetometer;
+    }
+
+    ekfSetStateSize(m_state_size);
+    m_x.resize(m_state_size);
+    serializeStateVector();
+    ekfSetOutputSize(m_output_size);
+
+    m_y.resize(m_output_size);
+    m_input_size = input_dimensions;
+    ekfSetInputSize(m_input_size);
+    m_u.resize(m_input_size);
+
+    // ekfinit resizes and initializes the filter matrices and vectors to zero - dont move it frome here
+    if (!ekfInit())
+    {
+        return false;
+    }
+
+    // once the buffers are resized and zeroed, setup the matrices
+    if (!setInitialStateCovariance(m_params.initial_orientation_error_variance,
+                                   m_params.initial_ang_vel_error_variance,
+                                   m_params.initial_gyro_bias_error_variance))
+    {
+        return false;
+    }
+
+    if (!setSystemNoiseVariance(m_params.gyroscope_noise_variance,
+                            m_params.gyro_bias_noise_variance))
+    {
+        return false;
+    }
+
+    if (!setMeasurementNoiseVariance(m_params.accelerometer_noise_variance,
+                                m_params.magnetometer_noise_variance))
+    {
+        return false;
+    }
+
+    m_initialized = true;
+    return m_initialized;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::propagateStates()
+{
+    iDynTree::Span<double> Omega_y_span(m_Omega_y.data(), m_Omega_y.size());
+    bool ok  = ekfSetInputVector(Omega_y_span);
+    ok = ekfPredict() && ok;
+    iDynTree::Span<double> x_span(m_x.data(), m_x.size());
+    if (ekfGetStates(x_span))
+    {
+        deserializeStateVector();
+        m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation);
+        m_orientationInRPY = m_orientationInSO3.asRPY();
+    }
+    else
+    {
+        iDynTree::reportError("AttitudeQuaternionEKF", "updateFilterWithMeasurements", "could not get recent state estimate");
+        return false;
+    }
+    return ok;
+}
+
+void iDynTree::AttitudeQuaternionEKF::serializeMeasurementVector()
+{
+    using iDynTree::toEigen;
+    if (m_y.size() != m_output_size)
+    {
+        m_y.resize(m_output_size);
+    }
+
+    toEigen(m_y).block<3, 1>(0, 0) = toEigen(m_Acc_y);
+    if (m_params.use_magnetometer_measurements)
+    {
+        m_y(3) = m_Mag_y;
+    }
+}
+
+bool iDynTree::AttitudeQuaternionEKF::callEkfUpdate()
+{
+    serializeMeasurementVector();
+    iDynTree::Span<double> y_span(m_y.data(), m_y.size());
+    bool ok = ekfSetMeasurementVector(y_span);
+    ok = ekfUpdate() && ok;
+
+    iDynTree::Span<double> x_span(m_x.data(), m_x.size());
+    if (ekfGetStates(x_span))
+    {
+        deserializeStateVector();
+        m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation);
+        m_orientationInRPY = m_orientationInSO3.asRPY();
+    }
+    else
+    {
+        iDynTree::reportError("AttitudeQuaternionEKF", "updateFilterWithMeasurements", "could not get recent state estimate");
+        return false;
+    }
+    return ok;
+}
+
+
+bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas, const iDynTree::MagnetometerMeasurements& magMeas)
+{
+    if (m_y.size() != m_output_size)
+    {
+        m_y.resize(m_output_size);
+    }
+    m_Acc_y = linAccMeas;
+    toEigen(m_Acc_y).normalize();
+    m_Omega_y = gyroMeas;
+
+    // compute yaw angle from magnetometer measurements by limiting the vertical influence of magentometer
+    // to do this first rotate the measurement to inertial frame, set z-component to zero.
+    // convert back to body frame and compute the yaw using atan2(y, x)
+    iDynTree::Vector3 mag_meas_in_inertial_frame;
+    iDynTree::Vector3 modified_mag_meas_in_body_frame;
+
+    using iDynTree::toEigen;
+    auto m_A(toEigen(mag_meas_in_inertial_frame));
+    auto m_B_modified(toEigen(modified_mag_meas_in_body_frame));
+    auto m_yB(toEigen(magMeas));
+    auto A_R_B(toEigen(m_orientationInSO3));
+
+    m_A = A_R_B * m_yB;
+    m_A(2) = 0; // to limit the vertical influence of magnetometer {^A}m_z = 0
+    m_B_modified = A_R_B.transpose()* m_A; // rotate vector back to body frame
+    m_Mag_y = atan2(-m_B_modified(1), m_B_modified(0));
+
+    // set accelerometer and magnetometer measurement
+    if (!callEkfUpdate())
+    {
+        return false;
+    }
+
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas)
+{
+    iDynTree::MagnetometerMeasurements magMeas;
+    magMeas.zero();
+    bool ok = updateFilterWithMeasurements(linAccMeas, gyroMeas, magMeas);
+    return ok;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F)
+{
+    using iDynTree::toEigen;
+
+    if (x.size() != m_state_size)
+    {
+        reportError("AttitudeQuaternionEKF", "computejacobianF", "state size mismatch");
+        return false;
+    }
+
+    if (F.rows() != m_state_size || F.cols() != m_state_size)
+    {
+        reportError("AttitudeQuaternionEKF", "computejacobianF", "jacobian matrix size mismatch");
+        return false;
+    }
+
+    F.zero();
+
+    iDynTree::UnitQuaternion q;
+    iDynTree::Vector3 ang_vel, gyro_bias;
+    toEigen(q) = toEigen(x).block<4,1>(0, 0);
+    toEigen(ang_vel) = toEigen(x).block<3,1>(4, 0);
+    toEigen(gyro_bias) = toEigen(x).block<3,1>(7, 0);
+
+    iDynTree::Matrix4x4 dfq_by_dq;
+    dfq_by_dq(0,0) = dfq_by_dq(1, 1) = dfq_by_dq(2, 2) = dfq_by_dq(3, 3) = (2.0/m_params.time_step_in_seconds);
+    dfq_by_dq(1, 0) = dfq_by_dq(2, 3) = ang_vel(0);
+    dfq_by_dq(2, 0) = dfq_by_dq(3, 1) = ang_vel(1);
+    dfq_by_dq(3, 0) = dfq_by_dq(1, 2) = ang_vel(2);
+    dfq_by_dq(0, 1) = dfq_by_dq(3, 2) = -ang_vel(0);
+    dfq_by_dq(0, 2) = dfq_by_dq(1, 3) = -ang_vel(1);
+    dfq_by_dq(0, 3) = dfq_by_dq(2, 1) = -ang_vel(2);
+    toEigen(dfq_by_dq) *= (m_params.time_step_in_seconds/2.0);
+
+    iDynTree::MatrixDynSize dfq_by_dangvel;
+    dfq_by_dangvel.resize(4, 3);
+    dfq_by_dangvel(0, 0) = dfq_by_dangvel(2, 2) = -q(1);
+    dfq_by_dangvel(0, 1) = dfq_by_dangvel(3, 0) = -q(2);
+    dfq_by_dangvel(0, 2) = dfq_by_dangvel(1, 1) = -q(3);
+    dfq_by_dangvel(2, 0) = q(3);
+    dfq_by_dangvel(1, 2) = q(2);
+    dfq_by_dangvel(3, 1) = q(1);
+    dfq_by_dangvel(1, 0) = dfq_by_dangvel(2, 1) = dfq_by_dangvel(3, 2) = -q(0);
+    toEigen(dfq_by_dangvel) *= (m_params.time_step_in_seconds/2.0);
+
+    iDynTree::Matrix3x3 dfangvel_by_dgyrobias(m_Id3);
+    toEigen(dfangvel_by_dgyrobias) *= -1;
+
+    iDynTree::Matrix3x3 dfgyrobias_by_dgyrobias(m_Id3);
+    toEigen(dfgyrobias_by_dgyrobias) *= (1 - (m_params.bias_correlation_time_factor*m_params.time_step_in_seconds));
+
+    toEigen(F).block<4,4>(0,0) = toEigen(dfq_by_dq);
+    toEigen(F).block<4,3>(0,4) = toEigen(dfq_by_dangvel);
+    toEigen(F).block<3,3>(4,7) = toEigen(dfangvel_by_dgyrobias);
+    toEigen(F).block<3,3>(7,7) = toEigen(dfgyrobias_by_dgyrobias);
+
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianH(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& H)
+{
+    using iDynTree::toEigen;
+
+    if (x.size() != m_state_size)
+    {
+        reportError("AttitudeQuaternionEKF", "computejacobianH", "state size mismatch");
+        return false;
+    }
+
+    if (H.rows() != m_output_size || H.cols() != m_state_size)
+    {
+        reportError("AttitudeQuaternionEKF", "computejacobianH", "jacobian matrix size mismatch");
+        return false;
+    }
+
+    H.zero();
+    iDynTree::UnitQuaternion q;
+    toEigen(q) = toEigen(x).block<4,1>(0, 0);
+
+    iDynTree::MatrixDynSize dhacc_by_dq;
+    dhacc_by_dq.resize(3, 4);
+    dhacc_by_dq(0, 0) = dhacc_by_dq(2, 2) = q(2);
+    dhacc_by_dq(0, 1) = dhacc_by_dq(1, 2) = -q(3);
+    dhacc_by_dq(0, 3) = dhacc_by_dq(1, 0) = -q(1);
+    dhacc_by_dq(1, 1) = dhacc_by_dq(2, 0) = -q(0);
+    dhacc_by_dq(0, 2) = q(0);
+    dhacc_by_dq(1, 3) = -q(2);
+    dhacc_by_dq(2, 1) = q(1);
+    dhacc_by_dq(2, 3) = -q(3);
+
+    if (m_gravity_direction(2) == -1)
+    {
+        toEigen(dhacc_by_dq) *= 2;
+    }
+    else if (m_gravity_direction(2) == 1)
+    {
+        toEigen(dhacc_by_dq) *= -2;
+    }
+    else
+    {
+        reportError("AttitudeQuaternionEKF", "computejacobianH", "filter assumes gravity pointing upward or downward only");
+        return false;
+    }
+
+    toEigen(H).block<3, 4>(0, 0) = toEigen(dhacc_by_dq);
+
+    if (m_output_size == output_dimensions_with_magnetometer)
+    {
+        double q0q3{q(0)*q(3)};
+        double q1q2{q(1)*q(2)};
+        double q2squared{q(2)*q(2)};
+        double q3squared{q(3)*q(3)};
+        double common_factor{(1 - 2*(q2squared + q3squared))};
+        double common_factorSquared{common_factor*common_factor};
+        double q0q3plusq1q2Squared{((q0q3+q1q2)*(q0q3+q1q2))};
+        double denominator{(4*q0q3plusq1q2Squared) + (common_factorSquared)};
+
+        double multiple{(2*common_factor/denominator)};
+        double factor{(-8*(q0q3 + q1q2))/denominator};
+
+        H(3, 0) = q(3)*multiple;
+        H(3, 1) = q(2)*multiple;
+        H(3, 1) = (q(1)*multiple) - (q(2)*factor);
+        H(3, 2) = (q(0)*multiple) - (q(3)*factor);
+    }
+
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::ekf_f(const iDynTree::VectorDynSize& x_k, const iDynTree::VectorDynSize& u_k, iDynTree::VectorDynSize& xhat_k_plus_one)
+{
+    if (x_k.size() != m_x.size() || xhat_k_plus_one.size() != m_x.size())
+    {
+        reportError("AttitudeQuaternionEKF", "f", "state size mismatch");
+        return false;
+    }
+
+    if (u_k.size() != m_u.size())
+    {
+        reportError("AttitudeQuaternionEKF", "f", "input size mismatch");
+        return false;
+    }
+
+    iDynTree::UnitQuaternion orientation;
+    iDynTree::Vector3 ang_vel, gyro_bias;
+
+    using iDynTree::toEigen;
+    auto q(toEigen(orientation));
+    auto Omega(toEigen(ang_vel));
+    auto b(toEigen(gyro_bias));
+    auto x(toEigen(x_k));
+    auto u(toEigen(u_k));
+    auto x_hat_plus(toEigen(xhat_k_plus_one));
+
+    q = x.block<4,1>(0, 0);
+    Omega = x.block<3,1>(4, 0);
+    b = x.block<3,1>(7, 0);
+
+    iDynTree::UnitQuaternion correction = pureQuaternion(ang_vel);
+    auto dq(toEigen(composeQuaternion2(orientation, correction)));
+
+    x_hat_plus.block<4,1>(0, 0) = q + (dq*(m_params.time_step_in_seconds*0.5));
+    x_hat_plus.block<3,1>(4, 0) = u - b;
+    x_hat_plus.block<3,1>(7, 0) = b*(1 - (m_params.bias_correlation_time_factor*m_params.time_step_in_seconds));
+
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one, iDynTree::VectorDynSize& zhat_k_plus_one)
+{
+    if (xhat_k_plus_one.size() != m_x.size())
+    {
+        reportError("AttitudeQuaternionEKF", "h", "predicted state size mismatch");
+        return false;
+    }
+
+    if (zhat_k_plus_one.size() != m_y.size())
+    {
+        reportError("AttitudeQuaternionEKF", "h", "output size mismatch");
+        return false;
+    }
+
+    // following computation is the same as R^T e_3
+    iDynTree::UnitQuaternion q;
+    toEigen(q) = toEigen(xhat_k_plus_one).block<4,1>(0, 0);
+    double q1q3{q(1)*q(3)};
+    double q0q2{q(0)*q(2)};
+    double q2q3{q(2)*q(3)};
+    double q0q1{q(0)*q(1)};
+    double q0squared{q(0)*q(0)};
+    double q1squared{q(1)*q(1)};
+    double q2squared{q(2)*q(2)};
+    double q3squared{q(3)*q(3)};
+
+    zhat_k_plus_one(0) = -2*(q1q3 - q0q2);
+    zhat_k_plus_one(1) = -2*(q2q3 + q0q1);
+    zhat_k_plus_one(2) = -(q0squared - q1squared - q2squared + q3squared);
+    if (m_gravity_direction(2) == -1)
+    {
+        // have same zhat
+    }
+    else if (m_gravity_direction(2) == 1)
+    {
+        toEigen(zhat_k_plus_one) *= -1;
+    }
+    else
+    {
+        reportError("AttitudeQuaternionEKF", "h", "filter assumes gravity pointing upward or downward only");
+        return false;
+    }
+
+    // magnetometer measurement gives us yaw
+    if (m_output_size == output_dimensions_with_magnetometer)
+    {
+        double q0q3{q(0)*q(3)};
+        double q1q2{q(1)*q(2)};
+        zhat_k_plus_one(3) = atan2(2*(q0q3+q1q2), 1 - 2*(q2squared + q3squared));
+    }
+
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::setMeasurementNoiseVariance(double acc, double mag)
+{
+    m_params.accelerometer_noise_variance = acc;
+    m_params.magnetometer_noise_variance = mag;
+
+    iDynTree::MatrixDynSize R = iDynTree::MatrixDynSize(m_output_size, m_output_size);
+    prepareMeasurementNoiseCovarianceMatrix(R);
+
+    iDynTree::Span<double> R_span(R.data(), R.capacity());
+    bool ok = ekfSetMeasurementNoiseCovariance(R_span);
+    return ok;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::setSystemNoiseVariance(double gyro, double gyro_bias)
+{
+    m_params.gyroscope_noise_variance = gyro;
+    m_params.gyro_bias_noise_variance = gyro_bias;
+
+    iDynTree::MatrixDynSize Q = iDynTree::MatrixDynSize(m_state_size, m_state_size);
+    prepareSystemNoiseCovarianceMatrix(Q);
+    iDynTree::Span<double> Q_span(Q.data(), Q.capacity());
+    bool ok = ekfSetSystemNoiseCovariance(Q_span);
+    return ok;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::setInitialStateCovariance(double orientation_var, double ang_vel_var, double gyro_bias_var)
+{
+    using iDynTree::toEigen;
+
+    iDynTree::MatrixDynSize P(m_state_size, m_state_size);
+
+    auto P_eig(toEigen(P));
+    auto Id3(toEigen(m_Id3));
+    auto Id4(toEigen(m_Id4));
+
+    P_eig.block<4,4>(0,0) = Id4*orientation_var;
+    P_eig.block<3,3>(4,4) = Id3*ang_vel_var;
+    P_eig.block<3,3>(7,7) = Id3*gyro_bias_var;
+
+    iDynTree::Span<double> P_span(P.data(), P.capacity());
+    bool ok = ekfSetStateCovariance(P_span);
+    return ok;
+}
+
+
+bool iDynTree::AttitudeQuaternionEKF::getDefaultInternalInitialState(const iDynTree::Span< double >& stateBuffer) const
+{
+    stateBuffer(0) = m_initial_state.m_orientation(0);
+    stateBuffer(1) = m_initial_state.m_orientation(1);
+    stateBuffer(2) = m_initial_state.m_orientation(2);
+    stateBuffer(3) = m_initial_state.m_orientation(3);
+    stateBuffer(4) = m_initial_state.m_angular_velocity(0);
+    stateBuffer(5) = m_initial_state.m_angular_velocity(1);
+    stateBuffer(6) = m_initial_state.m_angular_velocity(2);
+    stateBuffer(7) = m_initial_state.m_gyroscope_bias(0);
+    stateBuffer(8) = m_initial_state.m_gyroscope_bias(1);
+    stateBuffer(9) = m_initial_state.m_gyroscope_bias(2);
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::getInternalState(const iDynTree::Span< double >& stateBuffer) const
+{
+    stateBuffer(0) = m_state.m_orientation(0);
+    stateBuffer(1) = m_state.m_orientation(1);
+    stateBuffer(2) = m_state.m_orientation(2);
+    stateBuffer(3) = m_state.m_orientation(3);
+    stateBuffer(4) = m_state.m_angular_velocity(0);
+    stateBuffer(5) = m_state.m_angular_velocity(1);
+    stateBuffer(6) = m_state.m_angular_velocity(2);
+    stateBuffer(7) = m_state.m_gyroscope_bias(0);
+    stateBuffer(8) = m_state.m_gyroscope_bias(1);
+    stateBuffer(9) = m_state.m_gyroscope_bias(2);
+    return true;
+}
+
+std::size_t iDynTree::AttitudeQuaternionEKF::getInternalStateSize() const
+{
+    size_t size = 0;
+    size += m_state.m_orientation.size();
+    size += m_state.m_angular_velocity.size();
+    size += m_state.m_gyroscope_bias.size();
+
+    return size;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q)
+{
+    q = m_state.m_orientation;
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::getOrientationEstimateAsRotationMatrix(iDynTree::Rotation& rot)
+{
+    rot = m_orientationInSO3;
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::getOrientationEstimateAsRPY(iDynTree::RPY& rpy)
+{
+    rpy = m_orientationInRPY;
+    return true;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::setInternalState(const iDynTree::Span< double >& stateBuffer)
+{
+    if ((size_t)stateBuffer.size() != getInternalStateSize())
+    {
+        iDynTree::reportError("AttitudeQuaternionEKF", "setInternalState", "state size mismatch, using default state");
+        return false;
+    }
+    m_state.m_orientation(0) = stateBuffer(0);
+    m_state.m_orientation(1) = stateBuffer(1);
+    m_state.m_orientation(2) = stateBuffer(2);
+    m_state.m_orientation(3) = stateBuffer(3);
+    m_state.m_angular_velocity(0) = stateBuffer(4);
+    m_state.m_angular_velocity(1) = stateBuffer(5);
+    m_state.m_angular_velocity(2) = stateBuffer(6);
+    m_state.m_gyroscope_bias(0) = stateBuffer(7);
+    m_state.m_gyroscope_bias(1) = stateBuffer(8);
+    m_state.m_gyroscope_bias(2) = stateBuffer(9);
+
+    m_initial_state = m_state;
+    serializeStateVector();
+    iDynTree::Span<double> x0_span(m_x.data(), m_x.size());
+    bool ok = ekfSetInitialState(x0_span);
+    return ok;
+}
+
+bool iDynTree::AttitudeQuaternionEKF::setInternalStateInitialOrientation(const iDynTree::Span< double >& orientationBuffer)
+{
+    if ((size_t)orientationBuffer.size() != m_state.m_orientation.size())
+    {
+        iDynTree::reportError("AttitudeQuaternionEKF", "setInternalStateInitialOrientation", "orientation size mismatch, using default state");
+        return false;
+    }
+    m_state.m_orientation(0) = orientationBuffer(0);
+    m_state.m_orientation(1) = orientationBuffer(1);
+    m_state.m_orientation(2) = orientationBuffer(2);
+    m_state.m_orientation(3) = orientationBuffer(3);
+
+    return true;
+}
+
+
+bool iDynTree::AttitudeQuaternionEKF::useMagnetometerMeasurements(bool use_magnetometer_measurements)
+{
+    m_params.use_magnetometer_measurements = use_magnetometer_measurements;
+    ekfReset();
+
+    bool ok = initializeFilter();
+    iDynTree::Span<double> x_span(m_x.data(), m_x.size());
+    ok = setInternalState(x_span) && ok;
+    return ok;
+}
+
+void iDynTree::AttitudeQuaternionEKF::setGravityDirection(const iDynTree::Direction& gravity_dir)
+{
+    m_gravity_direction = gravity_dir;
+}
+
diff --git a/src/estimation/src/ExtendedKalmanFilter.cpp b/src/estimation/src/ExtendedKalmanFilter.cpp
new file mode 100644
index 00000000000..dd6fc6ddb7f
--- /dev/null
+++ b/src/estimation/src/ExtendedKalmanFilter.cpp
@@ -0,0 +1,286 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/ExtendedKalmanFilter.h>
+
+iDynTree::DiscreteExtendedKalmanFilterHelper::DiscreteExtendedKalmanFilterHelper()
+{
+
+}
+
+void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset()
+{
+    m_is_initialized = false;
+    m_measurement_updated = false;
+    m_input_updated = false;
+    m_initial_state_set = false;
+    m_initial_state_covariance_set = false;
+}
+
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit()
+{
+    if (m_dim_X == 0 || m_dim_Y == 0 || m_dim_U == 0)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "init", "state or input or output size not set, exiting.");
+        return false;
+    }
+
+    // resize state related quantities
+    m_x.resize(m_dim_X); m_x.zero();
+    m_xhat.resize(m_dim_X); m_xhat.zero();
+    m_u.resize(m_dim_U); m_u.zero();
+    m_y.resize(m_dim_Y); m_y.zero();
+
+    m_F.resize(m_dim_X, m_dim_X);
+    m_F.zero();
+    m_P.resize(m_dim_X, m_dim_X);
+    m_P.zero();
+    m_Phat.resize(m_dim_X, m_dim_X);
+    m_Phat.zero();
+    m_Q.resize(m_dim_X, m_dim_X);
+    m_Q.zero();
+
+    m_H.resize(m_dim_Y, m_dim_X);
+    m_H.zero();
+    m_S.resize(m_dim_Y, m_dim_Y);
+    m_S.zero();
+    m_K.resize(m_dim_X, m_dim_Y);
+    m_K.zero();
+    m_R.resize(m_dim_Y, m_dim_Y);
+    m_R.zero();
+
+    m_is_initialized = true;
+    return m_is_initialized;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfPredict()
+{
+    if (!m_is_initialized)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfPredict", "filter not initialized.");
+        return false;
+    }
+
+    if (!m_initial_state_set)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfPredict", "initial state not set.");
+        return false;
+    }
+
+    if (!m_initial_state_covariance_set)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfPredict", "initial state covariance not set.");
+        return false;
+    }
+
+    if (!m_input_updated)
+    {
+        iDynTree::reportWarning("DiscreteExtendedKalmanFilterHelper", "ekfPredict", "input not updated. using old input");
+    }
+
+    // propagate states and compute jacobian
+    ekf_f(m_x, m_u, m_xhat);                      ///< \f$ \hat{x}_{k+1} = f(x_k, u_k) \f$
+    ekfComputeJacobianF(m_x, m_F);               ///< \f$ F \mid_{x = x_k} \f$
+
+    using iDynTree::toEigen;
+    auto P(toEigen(m_P));
+    auto Phat(toEigen(m_Phat));
+    auto F(toEigen(m_F));
+    auto Q(toEigen(m_Q));
+
+    // propagate covariance
+    Phat = F*P*(F.transpose()) + Q;           ///< \f$ \hat{P}_{k+1} = F_k P_k F_k^T + Q \f$
+    m_input_updated = false;
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfUpdate()
+{
+    if (!m_is_initialized)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfUpdate", "filter not initialized.");
+        return false;
+    }
+
+    if (!m_initial_state_set)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfUpdate", "initial state not set.");
+        return false;
+    }
+
+    if (!m_initial_state_covariance_set)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfUpdate", "initial state covariance not set.");
+        return false;
+    }
+
+    if (!m_measurement_updated)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfUpdate", "measurements not updated.");
+        return false;
+    }
+
+    iDynTree::VectorDynSize z;
+    z.resize(m_dim_Y);
+    ekf_h(m_xhat, z);                             ///< \f$ z_{k+1} = h(\hat{x}_{k+1}) \f$
+    ekfComputeJacobianH(m_xhat, m_H);            ///< \f$ H \mid_{x = \hat{x}_{k+1}} \f$
+
+    using iDynTree::toEigen;
+    auto P(toEigen(m_P));
+    auto Phat(toEigen(m_Phat));
+    auto S(toEigen(m_S));
+    auto K(toEigen(m_K));
+    auto H(toEigen(m_H));
+    auto R(toEigen(m_R));
+    auto x(toEigen(m_x));
+    auto xhat(toEigen(m_xhat));
+    auto y(toEigen(m_y) - toEigen(z));        ///< innovation \f$ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} \f$
+
+    S = H*Phat*H.transpose() + R;             ///< \f$ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R \f$
+    K = Phat*H.transpose()*S.inverse();       ///< \f$ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} \f$
+    P = Phat - (K*H*Phat);                    ///< \f$ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) \f$
+    x = xhat + K*y;                           ///< \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} \f$
+
+    m_measurement_updated = false;
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetInitialState(const iDynTree::Span< double >& x0)
+{
+    if ((size_t)x0.size() != m_dim_X)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "setInitialState", "state size mismatch");
+        return false;
+    }
+
+    for (int i = 0; i < x0.size(); i++)
+    {
+        m_x(i) = x0(i);
+    }
+
+    m_initial_state_set = true;
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetInputVector(const iDynTree::Span< double >& u)
+{
+    if ((size_t)u.size() != m_dim_U)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfSetInputVector", "input size mismatch");
+        return false;
+    }
+
+    for (int i = 0; i < u.size(); i++)
+    {
+        m_u(i) = u(i);
+    }
+
+    m_input_updated = true;
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetMeasurementVector(const iDynTree::Span< double >& y)
+{
+    if ((size_t)y.size() != m_dim_Y)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfSetMeasurementVector", "measurement size mismatch");
+        return false;
+    }
+
+    for (int i = 0; i < y.size(); i++)
+    {
+        m_y(i) = y(i);
+    }
+
+    m_measurement_updated = true;
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetStateCovariance(const iDynTree::Span< double >& P)
+{
+    if ((size_t)P.size() != m_dim_X*m_dim_X)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "setSystemCovariance", "state covariance matrix size mismatch");
+        return false;
+    }
+
+    m_P = iDynTree::MatrixDynSize(P.data(), m_dim_X, m_dim_X);
+    m_initial_state_covariance_set = true;
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetSystemNoiseCovariance(const iDynTree::Span< double >& Q)
+{
+    if ((size_t)Q.size() != m_dim_X*m_dim_X)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "setSystemNoiseMeanAndCovariance", "noise covariance matrix size mismatch");
+        return false;
+    }
+
+    m_Q = iDynTree::MatrixDynSize(Q.data(), m_dim_X, m_dim_X);
+    return true;
+}
+
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfSetMeasurementNoiseCovariance(const iDynTree::Span< double >& R)
+{
+    if ((size_t)R.size() != m_dim_Y*m_dim_Y)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "setMeasurementNoiseMeanAndCovariance", "noise covariance matrix size mismatch");
+        return false;
+    }
+
+    m_R = iDynTree::MatrixDynSize(R.data(), m_dim_Y, m_dim_Y);
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStates(const iDynTree::Span< double >& x) const
+{
+    if ((size_t)x.size() != m_dim_X)
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfGetStates", "state size mismatch");
+        return false;
+    }
+
+    for (size_t i = 0; i < m_dim_X; i++)
+    {
+        x(i) = m_x(i);
+    }
+    return true;
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStateCovariance(const iDynTree::Span< double >& P) const
+{
+    if ((size_t)P.size() != m_P.capacity())
+    {
+        iDynTree::reportError("DiscreteExtendedKalmanFilterHelper", "ekfGetStateCovariance", "state covariance size mismatch");
+        return false;
+    }
+
+    // TODO: replace this really bad way of copying a matrix to a span
+    std::vector<double> temp;
+    temp.resize(P.size());
+    for (size_t i = 0; i < m_P.rows(); i++)
+    {
+        for (size_t j = 0; j < m_P.cols(); j++)
+        {
+            temp.push_back(m_P(i, j));
+        }
+    }
+
+    for (size_t k = 0; k < temp.size(); k++)
+    {
+        P(k) = temp[k];
+    }
+
+    return true;
+}
+
diff --git a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
new file mode 100644
index 00000000000..2f6f10ad9b7
--- /dev/null
+++ b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/AttitudeQuaternionEKF.h>
+#include <iDynTree/Estimation/AttitudeMahonyFilter.h>
+#include <iDynTree/Core/TestUtils.h>
+#include <iostream>
+#include <memory>
+void run(iDynTree::IAttitudeEstimator* estimator,
+         const iDynTree::LinAcceleration& acc,
+         const iDynTree::GyroscopeMeasurements& gyro,
+         const iDynTree::MagnetometerMeasurements& mag)
+{
+    std::cout << "Propagating states..." << std::endl;
+    estimator->propagateStates();
+    std::cout << "Update measurements..." << std::endl;
+    estimator->updateFilterWithMeasurements(acc, gyro, mag);
+}
+
+int main()
+{
+    std::unique_ptr<iDynTree::AttitudeQuaternionEKF> qEKF;
+
+    qEKF = std::make_unique<iDynTree::AttitudeQuaternionEKF>();
+    iDynTree::AttitudeQuaternionEKFParameters params;
+
+    size_t x_size = qEKF->getInternalStateSize();
+
+    // calling setParams resets and intializes the filter
+    qEKF->setParameters(params);
+    bool ok = qEKF->initializeFilter();
+    ASSERT_IS_TRUE(ok);
+    std::cout << "Propagate states will internally run EKF predict step" << std::endl;
+    std::cout << "if setParams() was not called before, calling propagateStates before setting internal state will throw initial state not set error...." << std::endl;
+    ok = qEKF->propagateStates();
+    ASSERT_IS_TRUE(ok);
+    std::cout << "Print.... OK" << std::endl;
+
+    iDynTree::VectorDynSize x0;
+    x0.resize(10);
+    x0.zero();
+    x0(0) = 0.5;
+    x0(1) = -0.5;
+    x0(2) = 0.5;
+    x0(3) = -0.5;
+
+    iDynTree::Span<double> x0_span(x0.data(), x0.size());
+    std::cout << "Setting initial internal state" << std::endl;
+    ok = qEKF->setInternalState(x0_span);
+    ASSERT_IS_TRUE(ok);
+
+    std::cout << "Calling use magnetometer method resets filter flags and re-initialized filter with current state as intial state" << std::endl;
+    qEKF->useMagnetometerMeasurements(true);
+    std::cout << "Since initial state is already set, calling propagate states will be succesful" << std::endl;
+    ok = qEKF->propagateStates();
+    ASSERT_IS_TRUE(ok);
+
+    iDynTree::LinAcceleration linAcc; linAcc.zero();
+    iDynTree::GyroscopeMeasurements gyro; gyro.zero();
+    iDynTree::MagnetometerMeasurements mag; mag.zero();
+    std::cout << "Update measurements will internally run EKF update step" << std::endl;
+    ok = qEKF->updateFilterWithMeasurements(linAcc, gyro, mag);
+    ASSERT_IS_TRUE(ok);
+
+    ok = qEKF->useMagnetometerMeasurements(false);
+    ASSERT_IS_TRUE(ok);
+    ok = qEKF->updateFilterWithMeasurements(linAcc, gyro);
+    ASSERT_IS_TRUE(ok);
+
+    iDynTree::IAttitudeEstimator* qekf_(qEKF.get());
+    for (int i = 0; i <10 ; i++ )
+    {
+        run(qekf_, linAcc, gyro, mag);
+    }
+
+    return EXIT_SUCCESS;
+}
diff --git a/src/estimation/tests/CMakeLists.txt b/src/estimation/tests/CMakeLists.txt
index 6f859688d85..ad1e7d8d718 100644
--- a/src/estimation/tests/CMakeLists.txt
+++ b/src/estimation/tests/CMakeLists.txt
@@ -27,3 +27,4 @@ add_estimation_test(BerdyMAPSolver)
 add_estimation_test(ExternalWrenchesEstimation)
 add_estimation_test(ExtWrenchesAndJointTorquesEstimator)
 add_estimation_test(SimpleLeggedOdometry)
+add_estimation_test(AttitudeEstimator)

From dfbe6ea17a0ee9bde9d34140a7e1a6d1daff13dd Mon Sep 17 00:00:00 2001
From: Prashanth Ramadoss <prashanthramadoss@gmail.com>
Date: Wed, 30 Jan 2019 10:18:36 +0100
Subject: [PATCH 095/194] [estimation] attitude estimator private headers fix

---
 .../include/iDynTree/Estimation/AttitudeMahonyFilter.h           | 1 -
 .../include/iDynTree/Estimation/AttitudeQuaternionEKF.h          | 1 -
 src/estimation/src/AttitudeMahonyFilter.cpp                      | 1 +
 src/estimation/src/AttitudeQuaternionEKF.cpp                     | 1 +
 4 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
index 113c40a8862..ece8984a2ea 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
@@ -12,7 +12,6 @@
 #define ATTITUDE_MAHONY_FILTER_H
 
 #include <iDynTree/Estimation/AttitudeEstimator.h>
-#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
 #include <iDynTree/Core/Direction.h>
 
 namespace iDynTree
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
index 7543ba663e0..7ce215bce2b 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
@@ -13,7 +13,6 @@
 
 #include <iDynTree/Estimation/AttitudeEstimator.h>
 #include <iDynTree/Estimation/ExtendedKalmanFilter.h>
-#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
 #include <iDynTree/Core/Direction.h>
 
 namespace iDynTree
diff --git a/src/estimation/src/AttitudeMahonyFilter.cpp b/src/estimation/src/AttitudeMahonyFilter.cpp
index 4d08dc93d83..05cd2fa6b89 100644
--- a/src/estimation/src/AttitudeMahonyFilter.cpp
+++ b/src/estimation/src/AttitudeMahonyFilter.cpp
@@ -9,6 +9,7 @@
  */
 
 #include <iDynTree/Estimation/AttitudeMahonyFilter.h>
+#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
 #include <ctime>
 
 iDynTree::Matrix3x3 getMatrixFromVectorVectorMultiplication(iDynTree::Vector3 a, iDynTree::Vector3 b)
diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp
index 0e466de809a..35209a01d87 100644
--- a/src/estimation/src/AttitudeQuaternionEKF.cpp
+++ b/src/estimation/src/AttitudeQuaternionEKF.cpp
@@ -9,6 +9,7 @@
  */
 
 #include <iDynTree/Estimation/AttitudeQuaternionEKF.h>
+#include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
 
 void iDynTree::AttitudeQuaternionEKF::serializeStateVector()
 {

From 09f29e25b11db79bc3949d131808e96b5fab0bee Mon Sep 17 00:00:00 2001
From: GiulioRomualdi <giulio.romualdi@gmail.com>
Date: Thu, 14 Mar 2019 14:51:22 +0100
Subject: [PATCH 096/194] implement getWorldFrameTransform() in
 SimpleLeggedOdometry class

---
 .../Estimation/SimpleLeggedOdometry.h         |  9 ++++++--
 src/estimation/src/SimpleLeggedOdometry.cpp   | 21 +++++++++++++++++++
 2 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
index 67f2fb0116f..defb1aac5d0 100644
--- a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
+++ b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
@@ -249,11 +249,16 @@ class SimpleLeggedOdometry
      * \note this method works only for link, not for arbitrary frames.
      */
     iDynTree::Transform getWorldLinkTransform(const LinkIndex frame_index);
+
+    /**
+     * Get the world_H_link transform for an arbitrary link.
+     *
+     * \note this method works also for arbitrary frames.
+     */
+    iDynTree::Transform getWorldFrameTransform(const LinkIndex frame_index);
 };
 
 
 } // End namespace iDynTree
 
 #endif // IDYNTREE_SIMPLE_LEGGED_ODOMETRY2_
-
-
diff --git a/src/estimation/src/SimpleLeggedOdometry.cpp b/src/estimation/src/SimpleLeggedOdometry.cpp
index fafcfcbe6b8..fb4187bf31b 100644
--- a/src/estimation/src/SimpleLeggedOdometry.cpp
+++ b/src/estimation/src/SimpleLeggedOdometry.cpp
@@ -297,6 +297,27 @@ Transform SimpleLeggedOdometry::getWorldLinkTransform(const LinkIndex link_index
     return m_world_H_fixedLink*base_H_fixed.inverse()*base_H_link;
 }
 
+Transform SimpleLeggedOdometry::getWorldFrameTransform(const LinkIndex frame_index)
+{
+    if( !this->m_kinematicsUpdated || !this->m_isOdometryInitialized  )
+    {
+        reportError("SimpleLeggedOdometry",
+                    "getWorldFrameTransform",
+                    "getWorldLinkTransform was called, but the kinematics update or the odometry init was never setted.");
+        return Transform::Identity();
+    }
 
+    if( !this->m_model.isValidFrameIndex(frame_index) )
+    {
+        reportError("SimpleLeggedOdometry",
+                    "getWorldFrameTransform",
+                    "getWorldLinkTransform was called, but the request linkindex is not part of the model");
+        return Transform::Identity();
+    }
+
+    LinkIndex linkIndex = this->m_model.getFrameLink(frame_index);
+    Transform fixedLink_H_fixedFrame = m_model.getFrameTransform(frame_index);
+    return getWorldLinkTransform(linkIndex) * fixedLink_H_fixedFrame;
 }
 
+}

From 2eb90cac5424f8e76a785a83fef6057fdac9c52d Mon Sep 17 00:00:00 2001
From: GiulioRomualdi <giulio.romualdi@gmail.com>
Date: Thu, 14 Mar 2019 14:52:17 +0100
Subject: [PATCH 097/194] implement changeFixedFrame() in SimpleLeggedOdometry
 class

---
 .../Estimation/SimpleLeggedOdometry.h         | 18 ++++++++
 src/estimation/src/SimpleLeggedOdometry.cpp   | 42 +++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
index defb1aac5d0..ce00dd2669f 100644
--- a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
+++ b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
@@ -233,6 +233,24 @@ class SimpleLeggedOdometry
      */
     bool changeFixedFrame(const FrameIndex newFixedFrame);
 
+    /**
+     * Change the link that the odometry assumes to be fixed
+     * with respect to the inertial/world frame.
+     *
+     * \note The position of the external frame is set by the user
+     */
+    bool changeFixedFrame(const std::string & newFixedFrame,
+                          const Transform & world_H_newFixedFrame);
+
+    /**
+     * Change the link that the odometry assumes to be fixed
+     * with respect to the inertial/world frame.
+     *
+     * \note The position of the external frame is set by the user
+     */
+    bool changeFixedFrame(const FrameIndex newFixedFrame,
+                          const Transform & world_H_newFixedFrame);
+
     /**
      * Get the link currently considered fixed with respect to the inertial frame.
      *
diff --git a/src/estimation/src/SimpleLeggedOdometry.cpp b/src/estimation/src/SimpleLeggedOdometry.cpp
index fb4187bf31b..6949c505175 100644
--- a/src/estimation/src/SimpleLeggedOdometry.cpp
+++ b/src/estimation/src/SimpleLeggedOdometry.cpp
@@ -257,6 +257,48 @@ bool SimpleLeggedOdometry::changeFixedFrame(const FrameIndex newFixedFrame)
     return true;
 }
 
+bool SimpleLeggedOdometry::changeFixedFrame(const FrameIndex newFixedFrame, const Transform & world_H_newFixedFrame)
+{
+    if( !this->m_kinematicsUpdated )
+    {
+        reportError("SimpleLeggedOdometry",
+                    "changeFixedFrame",
+                    "changeFixedFrame was called, but the kinematics info was never setted.");
+        return false;
+    }
+
+    LinkIndex newFixedLink = this->m_model.getFrameLink(newFixedFrame);
+
+    if( newFixedLink == LINK_INVALID_INDEX )
+    {
+        reportError("SimpleLeggedOdometry",
+                    "changeFixedFrame",
+                    "changeFixedFrame was called, but the provided new fixed frame is unknown.");
+        return false;
+    }
+
+    Transform newFixedFrame_H_newFixedLink = m_model.getFrameTransform(newFixedFrame).inverse();
+    this->m_world_H_fixedLink = world_H_newFixedFrame * newFixedFrame_H_newFixedLink;
+    this->m_fixedLinkIndex = newFixedLink;
+
+    return true;
+}
+
+bool SimpleLeggedOdometry::changeFixedFrame(const std::string& newFixedFrame, const Transform & world_H_newFixedFrame)
+{
+    iDynTree::FrameIndex newFixedFrameIndex = this->m_model.getFrameIndex(newFixedFrame);
+
+    if( newFixedFrameIndex == FRAME_INVALID_INDEX )
+    {
+        reportError("SimpleLeggedOdometry",
+                    "changeFixedFrame",
+                    "changeFixedFrame was called, but the provided new fixed frame is unknown.");
+        return false;
+    }
+
+    return this->changeFixedFrame(newFixedFrameIndex, world_H_newFixedFrame);
+}
+
 bool SimpleLeggedOdometry::changeFixedFrame(const std::string& newFixedFrame)
 {
     iDynTree::FrameIndex newFixedFrameIndex = this->m_model.getFrameIndex(newFixedFrame);

From 46e70416fe715b78f4fa7f1782118e42723d8a3e Mon Sep 17 00:00:00 2001
From: GiulioRomualdi <giulio.romualdi@gmail.com>
Date: Mon, 18 Mar 2019 11:21:43 +0100
Subject: [PATCH 098/194] fix misleading in getWorldFrameTransform() methos

---
 src/estimation/src/SimpleLeggedOdometry.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/estimation/src/SimpleLeggedOdometry.cpp b/src/estimation/src/SimpleLeggedOdometry.cpp
index 6949c505175..c479ce0fe5e 100644
--- a/src/estimation/src/SimpleLeggedOdometry.cpp
+++ b/src/estimation/src/SimpleLeggedOdometry.cpp
@@ -358,8 +358,8 @@ Transform SimpleLeggedOdometry::getWorldFrameTransform(const LinkIndex frame_ind
     }
 
     LinkIndex linkIndex = this->m_model.getFrameLink(frame_index);
-    Transform fixedLink_H_fixedFrame = m_model.getFrameTransform(frame_index);
-    return getWorldLinkTransform(linkIndex) * fixedLink_H_fixedFrame;
+    Transform link_H_frame = m_model.getFrameTransform(frame_index);
+    return getWorldLinkTransform(linkIndex) * link_H_frame;
 }
 
 }

From 7a4ee92ea35b39940874d30dc523cb739326653b Mon Sep 17 00:00:00 2001
From: GiulioRomualdi <giulio.romualdi@gmail.com>
Date: Mon, 18 Mar 2019 11:34:10 +0100
Subject: [PATCH 099/194] use FrameIndex instead of LinkIndex in
 getWorldFrameTransform()

---
 .../include/iDynTree/Estimation/SimpleLeggedOdometry.h        | 4 ++--
 src/estimation/src/SimpleLeggedOdometry.cpp                   | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
index ce00dd2669f..57d1fcdc960 100644
--- a/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
+++ b/src/estimation/include/iDynTree/Estimation/SimpleLeggedOdometry.h
@@ -269,11 +269,11 @@ class SimpleLeggedOdometry
     iDynTree::Transform getWorldLinkTransform(const LinkIndex frame_index);
 
     /**
-     * Get the world_H_link transform for an arbitrary link.
+     * Get the world_H_frame transform for an arbitrary frame.
      *
      * \note this method works also for arbitrary frames.
      */
-    iDynTree::Transform getWorldFrameTransform(const LinkIndex frame_index);
+    iDynTree::Transform getWorldFrameTransform(const FrameIndex frame_index);
 };
 
 
diff --git a/src/estimation/src/SimpleLeggedOdometry.cpp b/src/estimation/src/SimpleLeggedOdometry.cpp
index c479ce0fe5e..66b2ec68f63 100644
--- a/src/estimation/src/SimpleLeggedOdometry.cpp
+++ b/src/estimation/src/SimpleLeggedOdometry.cpp
@@ -339,7 +339,7 @@ Transform SimpleLeggedOdometry::getWorldLinkTransform(const LinkIndex link_index
     return m_world_H_fixedLink*base_H_fixed.inverse()*base_H_link;
 }
 
-Transform SimpleLeggedOdometry::getWorldFrameTransform(const LinkIndex frame_index)
+Transform SimpleLeggedOdometry::getWorldFrameTransform(const FrameIndex frame_index)
 {
     if( !this->m_kinematicsUpdated || !this->m_isOdometryInitialized  )
     {

From c23067e642d8cf8c8ea8abcb35df4194ba013b98 Mon Sep 17 00:00:00 2001
From: GiulioRomualdi <giulio.romualdi@gmail.com>
Date: Tue, 19 Mar 2019 16:38:21 +0100
Subject: [PATCH 100/194] update the changelog

---
 doc/releases/v0_12.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 22de63243b1..008a5097098 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -37,3 +37,5 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added `AttitudeMahonyFilter` implementation of an explicit formulation of passive complementary filter over quaternion groups
 * Added `AttitudeQuaternionEKF` implementation
 * All of the above addressed in the PR (https://github.com/robotology/idyntree/pull/516)
+* Added `getWorldFrameTransform` implementation in `SimpleLeggedOdometry` class
+* Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame

From d5bac2a7ffb8e0e131db20fb182bb9758beb84c8 Mon Sep 17 00:00:00 2001
From: Prashanth Ramadoss <prashanthramadoss@gmail.com>
Date: Tue, 26 Mar 2019 16:44:12 +0100
Subject: [PATCH 101/194] [AttitudeEstimator] [Span] adding fixes, features,
 bindings and tests  (#522)

---
 .travis.yml                                   |     4 +-
 bindings/iDynTree.i                           |    11 +
 .../+iDynTree/AccelerometerSensor.m           |    32 +-
 .../+iDynTree/ArticulatedBodyAlgorithm.m      |     2 +-
 .../ArticulatedBodyAlgorithmInternalBuffers.m |    44 +-
 .../+iDynTree/AttitudeEstimatorState.m        |    56 +
 .../+iDynTree/AttitudeMahonyFilter.m          |    78 +
 .../AttitudeMahonyFilterParameters.m          |    76 +
 .../+iDynTree/AttitudeQuaternionEKF.m         |    88 +
 .../AttitudeQuaternionEKFParameters.m         |   126 +
 .../+iDynTree/BerdyDynamicVariable.m          |    20 +-
 .../autogenerated/+iDynTree/BerdyHelper.m     |    64 +-
 .../autogenerated/+iDynTree/BerdyOptions.m    |    38 +-
 .../autogenerated/+iDynTree/BerdySensor.m     |    20 +-
 .../+iDynTree/BerdySparseMAPSolver.m          |    32 +-
 bindings/matlab/autogenerated/+iDynTree/Box.m |    18 +-
 .../matlab/autogenerated/+iDynTree/ColorViz.m |    20 +-
 .../+iDynTree/CompositeRigidBodyAlgorithm.m   |     2 +-
 .../ComputeLinearAndAngularMomentum.m         |     2 +-
 ...teLinearAndAngularMomentumDerivativeBias.m |     2 +-
 .../autogenerated/+iDynTree/ContactWrench.m   |    10 +-
 .../matlab/autogenerated/+iDynTree/Cylinder.m |    14 +-
 .../+iDynTree/DOFSpatialForceArray.m          |    10 +-
 .../+iDynTree/DOFSpatialMotionArray.m         |    10 +-
 .../+iDynTree/DOF_INVALID_INDEX.m             |     4 +-
 .../+iDynTree/DOF_INVALID_NAME.m              |     4 +-
 .../DiscreteExtendedKalmanFilterHelper.m      |    81 +
 .../autogenerated/+iDynTree/DynamicSpan.m     |    86 +
 .../+iDynTree/DynamicsComputations.m          |    72 +-
 .../+iDynTree/DynamicsRegressorGenerator.m    |    60 +-
 .../+iDynTree/DynamicsRegressorParameter.m    |    22 +-
 .../DynamicsRegressorParametersList.m         |    18 +-
 .../ExtWrenchesAndJointTorquesEstimator.m     |    28 +-
 .../autogenerated/+iDynTree/ExternalMesh.m    |    14 +-
 .../+iDynTree/FRAME_INVALID_INDEX.m           |     4 +-
 .../+iDynTree/FRAME_INVALID_NAME.m            |     4 +-
 .../autogenerated/+iDynTree/FixedJoint.m      |    62 +-
 .../+iDynTree/ForwardAccKinematics.m          |     2 +-
 .../+iDynTree/ForwardBiasAccKinematics.m      |     2 +-
 .../+iDynTree/ForwardPosVelAccKinematics.m    |     2 +-
 .../+iDynTree/ForwardPosVelKinematics.m       |     2 +-
 .../+iDynTree/ForwardPositionKinematics.m     |     2 +-
 .../+iDynTree/ForwardVelAccKinematics.m       |     2 +-
 .../+iDynTree/FrameFreeFloatingJacobian.m     |     8 +-
 .../autogenerated/+iDynTree/FreeFloatingAcc.m |    12 +-
 .../FreeFloatingGeneralizedTorques.m          |    12 +-
 .../+iDynTree/FreeFloatingMassMatrix.m        |     6 +-
 .../autogenerated/+iDynTree/FreeFloatingPos.m |    12 +-
 .../autogenerated/+iDynTree/FreeFloatingVel.m |    12 +-
 .../autogenerated/+iDynTree/GyroscopeSensor.m |    32 +-
 .../+iDynTree/IAttitudeEstimator.m            |    54 +
 .../matlab/autogenerated/+iDynTree/ICamera.m  |     8 +-
 .../autogenerated/+iDynTree/IEnvironment.m    |    18 +-
 .../+iDynTree/IJetsVisualization.m            |    16 +-
 .../matlab/autogenerated/+iDynTree/IJoint.m   |    68 +-
 .../matlab/autogenerated/+iDynTree/ILight.m   |    28 +-
 .../+iDynTree/IModelVisualization.m           |    38 +-
 .../+iDynTree/IVectorsVisualization.m         |    42 +
 ...verseDynamicsInertialParametersRegressor.m |     3 +
 .../+iDynTree/JOINT_INVALID_INDEX.m           |     4 +-
 .../+iDynTree/JOINT_INVALID_NAME.m            |     4 +-
 .../+iDynTree/JointDOFsDoubleArray.m          |     8 +-
 .../+iDynTree/JointPosDoubleArray.m           |     8 +-
 .../autogenerated/+iDynTree/JointSensor.m     |    12 +-
 .../+iDynTree/KinDynComputations.m            |   113 +-
 .../+iDynTree/LINK_INVALID_INDEX.m            |     4 +-
 .../+iDynTree/LINK_INVALID_NAME.m             |     4 +-
 .../matlab/autogenerated/+iDynTree/Link.m     |    14 +-
 .../autogenerated/+iDynTree/LinkAccArray.m    |    14 +-
 .../+iDynTree/LinkArticulatedBodyInertias.m   |    10 +-
 .../+iDynTree/LinkContactWrenches.m           |    18 +-
 .../autogenerated/+iDynTree/LinkInertias.m    |    10 +-
 .../autogenerated/+iDynTree/LinkPositions.m   |    14 +-
 .../autogenerated/+iDynTree/LinkSensor.m      |    16 +-
 .../+iDynTree/LinkUnknownWrenchContacts.m     |    22 +-
 .../autogenerated/+iDynTree/LinkVelArray.m    |    14 +-
 .../autogenerated/+iDynTree/LinkWrenches.m    |    16 +-
 .../matlab/autogenerated/+iDynTree/Model.m    |    78 +-
 .../autogenerated/+iDynTree/ModelLoader.m     |    24 +-
 .../+iDynTree/ModelParserOptions.m            |    12 +-
 .../+iDynTree/ModelSolidShapes.m              |    14 +-
 .../+iDynTree/MomentumFreeFloatingJacobian.m  |     8 +-
 .../+iDynTree/MovableJointImpl1.m             |    18 +-
 .../+iDynTree/MovableJointImpl2.m             |    18 +-
 .../+iDynTree/MovableJointImpl3.m             |    18 +-
 .../+iDynTree/MovableJointImpl4.m             |    18 +-
 .../+iDynTree/MovableJointImpl5.m             |    18 +-
 .../+iDynTree/MovableJointImpl6.m             |    18 +-
 .../+iDynTree/NR_OF_SENSOR_TYPES.m            |     2 +-
 .../matlab/autogenerated/+iDynTree/Neighbor.m |    12 +-
 .../autogenerated/+iDynTree/PrismaticJoint.m  |    50 +-
 .../+iDynTree/RNEADynamicPhase.m              |     2 +-
 .../autogenerated/+iDynTree/RevoluteJoint.m   |    50 +-
 .../matlab/autogenerated/+iDynTree/Sensor.m   |    18 +-
 .../autogenerated/+iDynTree/SensorsList.m     |    34 +-
 .../+iDynTree/SensorsMeasurements.m           |    18 +-
 .../+iDynTree/SimpleLeggedOdometry.m          |    22 +-
 .../+iDynTree/SixAxisForceTorqueSensor.m      |    60 +-
 .../autogenerated/+iDynTree/SolidShape.m      |    32 +-
 .../matlab/autogenerated/+iDynTree/Sphere.m   |    10 +-
 .../+iDynTree/TRAVERSAL_INVALID_INDEX.m       |     4 +-
 .../ThreeAxisAngularAccelerometerSensor.m     |    30 +-
 .../ThreeAxisForceTorqueContactSensor.m       |    36 +-
 .../autogenerated/+iDynTree/Traversal.m       |    34 +-
 .../+iDynTree/URDFParserOptions.m             |    12 +-
 .../+iDynTree/UnknownWrenchContact.m          |    24 +-
 .../autogenerated/+iDynTree/Visualizer.m      |    31 +-
 .../+iDynTree/VisualizerOptions.m             |    20 +-
 .../computeLinkNetWrenchesWithoutGravity.m    |     2 +-
 .../+iDynTree/dofsListFromURDF.m              |     2 +-
 .../+iDynTree/dofsListFromURDFString.m        |     2 +-
 .../autogenerated/+iDynTree/dynamic_extent.m  |     3 +
 ...ynamicsEstimationForwardVelAccKinematics.m |     2 +-
 .../dynamicsEstimationForwardVelKinematics.m  |     2 +-
 .../+iDynTree/estimateExternalWrenches.m      |     2 +-
 .../estimateExternalWrenchesBuffers.m         |    36 +-
 ...stimateExternalWrenchesWithoutInternalFT.m |     2 +-
 ...ntactWrenchesFromLinkNetExternalWrenches.m |     2 +-
 .../+iDynTree/getSensorTypeSize.m             |     2 +-
 .../+iDynTree/input_dimensions.m              |     3 +
 .../+iDynTree/isDOFBerdyDynamicVariable.m     |     2 +-
 .../+iDynTree/isJointBerdyDynamicVariable.m   |     2 +-
 .../autogenerated/+iDynTree/isJointSensor.m   |     2 +-
 .../+iDynTree/isLinkBerdyDynamicVariable.m    |     2 +-
 .../autogenerated/+iDynTree/isLinkSensor.m    |     2 +-
 .../autogenerated/+iDynTree/modelFromURDF.m   |     2 +-
 .../+iDynTree/modelFromURDFString.m           |     2 +-
 .../output_dimensions_with_magnetometer.m     |     3 +
 .../output_dimensions_without_magnetometer.m  |     3 +
 .../+iDynTree/predictSensorsMeasurements.m    |     2 +-
 ...predictSensorsMeasurementsFromRawBuffers.m |     2 +-
 .../autogenerated/+iDynTree/sensorsFromURDF.m |     2 +-
 .../+iDynTree/sensorsFromURDFString.m         |     2 +-
 .../autogenerated/iDynTreeMATLAB_wrap.cxx     | 44161 +++++++++-------
 bindings/matlab/tests/EKFTest.m               |    30 +
 doc/releases/v0_12.md                         |     7 +
 src/core/include/iDynTree/Core/Span.h         |   185 +-
 .../iDynTree/Estimation/AttitudeEstimator.h   |    15 +
 .../Estimation/AttitudeEstimatorUtils.h       |    35 +
 .../Estimation/AttitudeMahonyFilter.h         |    23 +-
 .../Estimation/AttitudeQuaternionEKF.h        |    36 +-
 .../Estimation/ExtendedKalmanFilter.h         |     2 +-
 src/estimation/src/AttitudeEstimatorUtils.cpp |    65 +-
 src/estimation/src/AttitudeMahonyFilter.cpp   |   188 +-
 src/estimation/src/AttitudeQuaternionEKF.cpp  |   234 +-
 src/estimation/src/ExtendedKalmanFilter.cpp   |    16 +-
 .../tests/AttitudeEstimatorUnitTest.cpp       |    48 +-
 147 files changed, 27643 insertions(+), 20159 deletions(-)
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
 create mode 100644 bindings/matlab/tests/EKFTest.m

diff --git a/.travis.yml b/.travis.yml
index 29b764b9b5d..a10f0197146 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -20,8 +20,8 @@ env:
   global:
     - TRAVIS_CMAKE_GENERATOR="Unix Makefiles"
   matrix:
-    - TRAVIS_BUILD_TYPE="Release" UBUNTU="xenial" VALGRIND_TESTS="ON" COMPILE_BINDINGS="ON" FULL_DEPS="ON"
-    - TRAVIS_BUILD_TYPE="Debug" UBUNTU="xenial" VALGRIND_TESTS="OFF" COMPILE_BINDINGS="ON" FULL_DEPS="ON"
+    - TRAVIS_BUILD_TYPE="Release" UBUNTU="xenial" VALGRIND_TESTS="ON" COMPILE_BINDINGS="OFF" FULL_DEPS="ON"
+    - TRAVIS_BUILD_TYPE="Debug" UBUNTU="xenial" VALGRIND_TESTS="OFF" COMPILE_BINDINGS="OFF" FULL_DEPS="ON"
     - TRAVIS_BUILD_TYPE="Release" UBUNTU="bionic" VALGRIND_TESTS="OFF" COMPILE_BINDINGS="ON" FULL_DEPS="ON"
     - TRAVIS_BUILD_TYPE="Debug" UBUNTU="bionic" VALGRIND_TESTS="OFF" COMPILE_BINDINGS="ON" FULL_DEPS="ON"
 
diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i
index 06f6853a58d..e47c756758d 100644
--- a/bindings/iDynTree.i
+++ b/bindings/iDynTree.i
@@ -71,6 +71,7 @@
 #include "iDynTree/Core/TransformSemantics.h"
 #include "iDynTree/Core/Transform.h"
 #include "iDynTree/Core/TransformDerivative.h"
+#include "iDynTree/Core/Span.h"
 
 // Model related data structures
 #include "iDynTree/Model/Indices.h"
@@ -114,6 +115,10 @@
 #include "iDynTree/Estimation/SimpleLeggedOdometry.h"
 #include "iDynTree/Estimation/BerdyHelper.h"
 #include "iDynTree/Estimation/BerdySparseMAPSolver.h"
+#include "iDynTree/Estimation/AttitudeEstimator.h"
+#include "iDynTree/Estimation/AttitudeMahonyFilter.h"
+#include "iDynTree/Estimation/ExtendedKalmanFilter.h"
+#include "iDynTree/Estimation/AttitudeQuaternionEKF.h"
 
 // Regressors related data structures
 #include "iDynTree/Regressors/DynamicsRegressorParameters.h"
@@ -256,7 +261,9 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 %include "iDynTree/Core/TransformSemantics.h"
 %include "iDynTree/Core/Transform.h"
 %include "iDynTree/Core/TransformDerivative.h"
+%include "iDynTree/Core/Span.h"
 
+%template(DynamicSpan) iDynTree::Span<double, iDynTree::dynamic_extent>;
 
 // Model related data structures
 %include "iDynTree/Model/Indices.h"
@@ -312,6 +319,10 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 %include "iDynTree/Estimation/SimpleLeggedOdometry.h"
 %include "iDynTree/Estimation/BerdyHelper.h"
 %include "iDynTree/Estimation/BerdySparseMAPSolver.h"
+%include "iDynTree/Estimation/AttitudeEstimator.h"
+%include "iDynTree/Estimation/AttitudeMahonyFilter.h"
+%include "iDynTree/Estimation/ExtendedKalmanFilter.h"
+%include "iDynTree/Estimation/AttitudeQuaternionEKF.h"
 
 // Regressors related data structures
 %include "iDynTree/Regressors/DynamicsRegressorParameters.h"
diff --git a/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m b/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m
index 90686875485..b6534e0eab2 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m
@@ -7,58 +7,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1345, varargin{:});
+        tmp = iDynTreeMEX(1369, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1346, self);
+        iDynTreeMEX(1370, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1347, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1371, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1348, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1372, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1349, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1373, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1350, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1374, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1351, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1375, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1352, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1376, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1353, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1377, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1354, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1378, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1355, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1379, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1356, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1380, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1357, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1381, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1358, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1382, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1359, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1383, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1360, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1384, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m
index 481c5eb2c6d..e6133cbd920 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m
@@ -1,3 +1,3 @@
 function varargout = ArticulatedBodyAlgorithm(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1261, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1284, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m
index c0f5a31d900..28497e05a4b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m
@@ -9,110 +9,110 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1239, varargin{:});
+        tmp = iDynTreeMEX(1262, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1240, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1263, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1241, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1264, self, varargin{:});
     end
     function varargout = S(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1242, self);
+        varargout{1} = iDynTreeMEX(1265, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1243, self, varargin{1});
+        iDynTreeMEX(1266, self, varargin{1});
       end
     end
     function varargout = U(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1244, self);
+        varargout{1} = iDynTreeMEX(1267, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1245, self, varargin{1});
+        iDynTreeMEX(1268, self, varargin{1});
       end
     end
     function varargout = D(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1246, self);
+        varargout{1} = iDynTreeMEX(1269, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1247, self, varargin{1});
+        iDynTreeMEX(1270, self, varargin{1});
       end
     end
     function varargout = u(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1248, self);
+        varargout{1} = iDynTreeMEX(1271, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1249, self, varargin{1});
+        iDynTreeMEX(1272, self, varargin{1});
       end
     end
     function varargout = linksVel(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1250, self);
+        varargout{1} = iDynTreeMEX(1273, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1251, self, varargin{1});
+        iDynTreeMEX(1274, self, varargin{1});
       end
     end
     function varargout = linksBiasAcceleration(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1252, self);
+        varargout{1} = iDynTreeMEX(1275, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1253, self, varargin{1});
+        iDynTreeMEX(1276, self, varargin{1});
       end
     end
     function varargout = linksAccelerations(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1254, self);
+        varargout{1} = iDynTreeMEX(1277, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1255, self, varargin{1});
+        iDynTreeMEX(1278, self, varargin{1});
       end
     end
     function varargout = linkABIs(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1256, self);
+        varargout{1} = iDynTreeMEX(1279, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1257, self, varargin{1});
+        iDynTreeMEX(1280, self, varargin{1});
       end
     end
     function varargout = linksBiasWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1258, self);
+        varargout{1} = iDynTreeMEX(1281, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1259, self, varargin{1});
+        iDynTreeMEX(1282, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1260, self);
+        iDynTreeMEX(1283, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
new file mode 100644
index 00000000000..36bf097e9ff
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
@@ -0,0 +1,56 @@
+classdef AttitudeEstimatorState < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function varargout = m_orientation(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1694, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1695, self, varargin{1});
+      end
+    end
+    function varargout = m_angular_velocity(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1696, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1697, self, varargin{1});
+      end
+    end
+    function varargout = m_gyroscope_bias(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1698, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1699, self, varargin{1});
+      end
+    end
+    function self = AttitudeEstimatorState(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1700, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1701, self);
+        self.SwigClear();
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
new file mode 100644
index 00000000000..0c88c1ddd1e
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
@@ -0,0 +1,78 @@
+classdef AttitudeMahonyFilter < iDynTree.IAttitudeEstimator
+  methods
+    function self = AttitudeMahonyFilter(varargin)
+      self@iDynTree.IAttitudeEstimator(SwigRef.Null);
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1651, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function varargout = useMagnetoMeterMeasurements(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1652, self, varargin{:});
+    end
+    function varargout = setConfidenceForMagnetometerMeasurements(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1653, self, varargin{:});
+    end
+    function varargout = setGainkp(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1654, self, varargin{:});
+    end
+    function varargout = setGainki(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1655, self, varargin{:});
+    end
+    function varargout = setTimeStepInSeconds(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1656, self, varargin{:});
+    end
+    function varargout = setGravityDirection(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1657, self, varargin{:});
+    end
+    function varargout = setParameters(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1658, self, varargin{:});
+    end
+    function varargout = getParameters(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1659, self, varargin{:});
+    end
+    function varargout = updateFilterWithMeasurements(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1660, self, varargin{:});
+    end
+    function varargout = propagateStates(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1661, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1662, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsQuaternion(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1663, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsRPY(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1664, self, varargin{:});
+    end
+    function varargout = getInternalStateSize(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1665, self, varargin{:});
+    end
+    function varargout = getInternalState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1666, self, varargin{:});
+    end
+    function varargout = getDefaultInternalInitialState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1667, self, varargin{:});
+    end
+    function varargout = setInternalState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1668, self, varargin{:});
+    end
+    function varargout = setInternalStateInitialOrientation(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1669, self, varargin{:});
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1670, self);
+        self.SwigClear();
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
new file mode 100644
index 00000000000..c59340d0177
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
@@ -0,0 +1,76 @@
+classdef AttitudeMahonyFilterParameters < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function varargout = time_step_in_seconds(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1639, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1640, self, varargin{1});
+      end
+    end
+    function varargout = kp(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1641, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1642, self, varargin{1});
+      end
+    end
+    function varargout = ki(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1643, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1644, self, varargin{1});
+      end
+    end
+    function varargout = use_magnetometer_measurements(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1645, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1646, self, varargin{1});
+      end
+    end
+    function varargout = confidence_magnetometer_measurements(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1647, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1648, self, varargin{1});
+      end
+    end
+    function self = AttitudeMahonyFilterParameters(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1649, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1650, self);
+        self.SwigClear();
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
new file mode 100644
index 00000000000..d229b8d6880
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
@@ -0,0 +1,88 @@
+classdef AttitudeQuaternionEKF < iDynTree.IAttitudeEstimator & iDynTree.DiscreteExtendedKalmanFilterHelper
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function self = AttitudeQuaternionEKF(varargin)
+      self@iDynTree.IAttitudeEstimator(SwigRef.Null);
+      self@iDynTree.DiscreteExtendedKalmanFilterHelper(SwigRef.Null);
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1724, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function varargout = getParameters(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1725, self, varargin{:});
+    end
+    function varargout = setParameters(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1726, self, varargin{:});
+    end
+    function varargout = setGravityDirection(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1727, self, varargin{:});
+    end
+    function varargout = setTimeStepInSeconds(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1728, self, varargin{:});
+    end
+    function varargout = setBiasCorrelationTimeFactor(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1729, self, varargin{:});
+    end
+    function varargout = useMagnetometerMeasurements(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1730, self, varargin{:});
+    end
+    function varargout = setMeasurementNoiseVariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1731, self, varargin{:});
+    end
+    function varargout = setSystemNoiseVariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1732, self, varargin{:});
+    end
+    function varargout = setInitialStateCovariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1733, self, varargin{:});
+    end
+    function varargout = initializeFilter(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1734, self, varargin{:});
+    end
+    function varargout = updateFilterWithMeasurements(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1735, self, varargin{:});
+    end
+    function varargout = propagateStates(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1736, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1737, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsQuaternion(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1738, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsRPY(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1739, self, varargin{:});
+    end
+    function varargout = getInternalStateSize(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1740, self, varargin{:});
+    end
+    function varargout = getInternalState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1741, self, varargin{:});
+    end
+    function varargout = getDefaultInternalInitialState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1742, self, varargin{:});
+    end
+    function varargout = setInternalState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1743, self, varargin{:});
+    end
+    function varargout = setInternalStateInitialOrientation(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1744, self, varargin{:});
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1745, self);
+        self.SwigClear();
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
new file mode 100644
index 00000000000..18752c94df2
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
@@ -0,0 +1,126 @@
+classdef AttitudeQuaternionEKFParameters < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function varargout = time_step_in_seconds(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1702, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1703, self, varargin{1});
+      end
+    end
+    function varargout = bias_correlation_time_factor(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1704, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1705, self, varargin{1});
+      end
+    end
+    function varargout = accelerometer_noise_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1706, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1707, self, varargin{1});
+      end
+    end
+    function varargout = magnetometer_noise_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1708, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1709, self, varargin{1});
+      end
+    end
+    function varargout = gyroscope_noise_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1710, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1711, self, varargin{1});
+      end
+    end
+    function varargout = gyro_bias_noise_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1712, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1713, self, varargin{1});
+      end
+    end
+    function varargout = initial_orientation_error_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1714, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1715, self, varargin{1});
+      end
+    end
+    function varargout = initial_ang_vel_error_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1716, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1717, self, varargin{1});
+      end
+    end
+    function varargout = initial_gyro_bias_error_variance(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1718, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1719, self, varargin{1});
+      end
+    end
+    function varargout = use_magnetometer_measurements(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1720, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1721, self, varargin{1});
+      end
+    end
+    function self = AttitudeQuaternionEKFParameters(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1722, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1723, self);
+        self.SwigClear();
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
index 83a6ed5ca57..0fb3cc533e0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
@@ -7,37 +7,37 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1546, self);
+        varargout{1} = iDynTreeMEX(1570, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1547, self, varargin{1});
+        iDynTreeMEX(1571, self, varargin{1});
       end
     end
     function varargout = id(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1548, self);
+        varargout{1} = iDynTreeMEX(1572, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1549, self, varargin{1});
+        iDynTreeMEX(1573, self, varargin{1});
       end
     end
     function varargout = range(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1550, self);
+        varargout{1} = iDynTreeMEX(1574, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1551, self, varargin{1});
+        iDynTreeMEX(1575, self, varargin{1});
       end
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1552, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1576, self, varargin{:});
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1553, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1577, self, varargin{:});
     end
     function self = BerdyDynamicVariable(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -45,14 +45,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1554, varargin{:});
+        tmp = iDynTreeMEX(1578, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1555, self);
+        iDynTreeMEX(1579, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m b/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
index 5c504ec5ab8..5259efa69fe 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
@@ -9,104 +9,104 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1556, varargin{:});
+        tmp = iDynTreeMEX(1580, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = dynamicTraversal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1557, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1581, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1558, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1582, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1559, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1583, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1560, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1584, self, varargin{:});
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1561, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1585, self, varargin{:});
     end
     function varargout = getOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1562, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1586, self, varargin{:});
     end
     function varargout = getNrOfDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1563, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1587, self, varargin{:});
     end
     function varargout = getNrOfDynamicEquations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1564, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1588, self, varargin{:});
     end
     function varargout = getNrOfSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1565, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1589, self, varargin{:});
     end
     function varargout = resizeAndZeroBerdyMatrices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1566, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1590, self, varargin{:});
     end
     function varargout = getBerdyMatrices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1567, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1591, self, varargin{:});
     end
     function varargout = getSensorsOrdering(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1568, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1592, self, varargin{:});
     end
     function varargout = getRangeSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1569, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1593, self, varargin{:});
     end
     function varargout = getRangeDOFSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1570, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1594, self, varargin{:});
     end
     function varargout = getRangeJointSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1571, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1595, self, varargin{:});
     end
     function varargout = getRangeLinkSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1572, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1596, self, varargin{:});
     end
     function varargout = getRangeLinkVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1573, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1597, self, varargin{:});
     end
     function varargout = getRangeJointVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1574, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1598, self, varargin{:});
     end
     function varargout = getRangeDOFVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1575, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1599, self, varargin{:});
     end
     function varargout = getDynamicVariablesOrdering(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1576, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1600, self, varargin{:});
     end
     function varargout = serializeDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1577, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1601, self, varargin{:});
     end
     function varargout = serializeSensorVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1578, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1602, self, varargin{:});
     end
     function varargout = serializeDynamicVariablesComputedFromFixedBaseRNEA(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1579, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1603, self, varargin{:});
     end
     function varargout = extractJointTorquesFromDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1580, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1604, self, varargin{:});
     end
     function varargout = extractLinkNetExternalWrenchesFromDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1581, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1605, self, varargin{:});
     end
     function varargout = updateKinematicsFromFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1582, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1606, self, varargin{:});
     end
     function varargout = updateKinematicsFromFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1583, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1607, self, varargin{:});
     end
     function varargout = updateKinematicsFromTraversalFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1584, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1608, self, varargin{:});
     end
     function varargout = setNetExternalWrenchMeasurementFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1585, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1609, self, varargin{:});
     end
     function varargout = getNetExternalWrenchMeasurementFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1586, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1610, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1587, self);
+        iDynTreeMEX(1611, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m b/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
index 656f0dd650b..9bdc6e1c612 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
@@ -9,7 +9,7 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1517, varargin{:});
+        tmp = iDynTreeMEX(1541, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
@@ -18,88 +18,88 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1518, self);
+        varargout{1} = iDynTreeMEX(1542, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1519, self, varargin{1});
+        iDynTreeMEX(1543, self, varargin{1});
       end
     end
     function varargout = includeAllNetExternalWrenchesAsDynamicVariables(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1520, self);
+        varargout{1} = iDynTreeMEX(1544, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1521, self, varargin{1});
+        iDynTreeMEX(1545, self, varargin{1});
       end
     end
     function varargout = includeAllJointAccelerationsAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1522, self);
+        varargout{1} = iDynTreeMEX(1546, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1523, self, varargin{1});
+        iDynTreeMEX(1547, self, varargin{1});
       end
     end
     function varargout = includeAllJointTorquesAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1524, self);
+        varargout{1} = iDynTreeMEX(1548, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1525, self, varargin{1});
+        iDynTreeMEX(1549, self, varargin{1});
       end
     end
     function varargout = includeAllNetExternalWrenchesAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1526, self);
+        varargout{1} = iDynTreeMEX(1550, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1527, self, varargin{1});
+        iDynTreeMEX(1551, self, varargin{1});
       end
     end
     function varargout = includeFixedBaseExternalWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1528, self);
+        varargout{1} = iDynTreeMEX(1552, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1529, self, varargin{1});
+        iDynTreeMEX(1553, self, varargin{1});
       end
     end
     function varargout = jointOnWhichTheInternalWrenchIsMeasured(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1530, self);
+        varargout{1} = iDynTreeMEX(1554, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1531, self, varargin{1});
+        iDynTreeMEX(1555, self, varargin{1});
       end
     end
     function varargout = baseLink(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1532, self);
+        varargout{1} = iDynTreeMEX(1556, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1533, self, varargin{1});
+        iDynTreeMEX(1557, self, varargin{1});
       end
     end
     function varargout = checkConsistency(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1534, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1558, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1535, self);
+        iDynTreeMEX(1559, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m b/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
index fbeaa2bad60..ef16db95a0b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
@@ -7,37 +7,37 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1536, self);
+        varargout{1} = iDynTreeMEX(1560, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1537, self, varargin{1});
+        iDynTreeMEX(1561, self, varargin{1});
       end
     end
     function varargout = id(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1538, self);
+        varargout{1} = iDynTreeMEX(1562, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1539, self, varargin{1});
+        iDynTreeMEX(1563, self, varargin{1});
       end
     end
     function varargout = range(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1540, self);
+        varargout{1} = iDynTreeMEX(1564, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1541, self, varargin{1});
+        iDynTreeMEX(1565, self, varargin{1});
       end
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1542, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1566, self, varargin{:});
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1543, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1567, self, varargin{:});
     end
     function self = BerdySensor(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -45,14 +45,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1544, varargin{:});
+        tmp = iDynTreeMEX(1568, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1545, self);
+        iDynTreeMEX(1569, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m b/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
index 2c663b2cb24..f7ba5878a7c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
@@ -9,58 +9,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1588, varargin{:});
+        tmp = iDynTreeMEX(1612, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1589, self);
+        iDynTreeMEX(1613, self);
         self.SwigClear();
       end
     end
     function varargout = setDynamicsConstraintsPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1590, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1614, self, varargin{:});
     end
     function varargout = setDynamicsRegularizationPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1591, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1615, self, varargin{:});
     end
     function varargout = setDynamicsRegularizationPriorExpectedValue(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1592, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1616, self, varargin{:});
     end
     function varargout = setMeasurementsPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1593, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1617, self, varargin{:});
     end
     function varargout = dynamicsConstraintsPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1594, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1618, self, varargin{:});
     end
     function varargout = dynamicsRegularizationPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1595, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1619, self, varargin{:});
     end
     function varargout = dynamicsRegularizationPriorExpectedValue(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1596, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1620, self, varargin{:});
     end
     function varargout = measurementsPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1597, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1621, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1598, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1622, self, varargin{:});
     end
     function varargout = initialize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1599, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1623, self, varargin{:});
     end
     function varargout = updateEstimateInformationFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1600, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1624, self, varargin{:});
     end
     function varargout = updateEstimateInformationFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1601, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1625, self, varargin{:});
     end
     function varargout = doEstimate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1602, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1626, self, varargin{:});
     end
     function varargout = getLastEstimate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1603, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1627, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/Box.m b/bindings/matlab/autogenerated/+iDynTree/Box.m
index d3741130d6f..7bb8d955905 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Box.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Box.m
@@ -2,41 +2,41 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1087, self);
+        iDynTreeMEX(1110, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1088, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1111, self, varargin{:});
     end
     function varargout = x(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1089, self);
+        varargout{1} = iDynTreeMEX(1112, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1090, self, varargin{1});
+        iDynTreeMEX(1113, self, varargin{1});
       end
     end
     function varargout = y(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1091, self);
+        varargout{1} = iDynTreeMEX(1114, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1092, self, varargin{1});
+        iDynTreeMEX(1115, self, varargin{1});
       end
     end
     function varargout = z(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1093, self);
+        varargout{1} = iDynTreeMEX(1116, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1094, self, varargin{1});
+        iDynTreeMEX(1117, self, varargin{1});
       end
     end
     function self = Box(varargin)
@@ -46,7 +46,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1095, varargin{:});
+        tmp = iDynTreeMEX(1118, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ColorViz.m b/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
index 78fcb4dd111..cb6bab3b622 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1710, self);
+        varargout{1} = iDynTreeMEX(1855, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1711, self, varargin{1});
+        iDynTreeMEX(1856, self, varargin{1});
       end
     end
     function varargout = g(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1712, self);
+        varargout{1} = iDynTreeMEX(1857, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1713, self, varargin{1});
+        iDynTreeMEX(1858, self, varargin{1});
       end
     end
     function varargout = b(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1714, self);
+        varargout{1} = iDynTreeMEX(1859, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1715, self, varargin{1});
+        iDynTreeMEX(1860, self, varargin{1});
       end
     end
     function varargout = a(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1716, self);
+        varargout{1} = iDynTreeMEX(1861, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1717, self, varargin{1});
+        iDynTreeMEX(1862, self, varargin{1});
       end
     end
     function self = ColorViz(varargin)
@@ -49,14 +49,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1718, varargin{:});
+        tmp = iDynTreeMEX(1863, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1719, self);
+        iDynTreeMEX(1864, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m b/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m
index 5ebc6270474..0ea796ae149 100644
--- a/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m
+++ b/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m
@@ -1,3 +1,3 @@
 function varargout = CompositeRigidBodyAlgorithm(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1238, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1261, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m
index 56c80610dec..9e14f6d4fed 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m
@@ -1,3 +1,3 @@
 function varargout = ComputeLinearAndAngularMomentum(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1235, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1258, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m
index 108fdfdde54..72efc51b0d5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m
@@ -1,3 +1,3 @@
 function varargout = ComputeLinearAndAngularMomentumDerivativeBias(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1236, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1259, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m b/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m
index 6ec6c1d0994..5639709d646 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m
@@ -4,13 +4,13 @@
       this = iDynTreeMEX(3, self);
     end
     function varargout = contactId(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1215, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1238, self, varargin{:});
     end
     function varargout = contactPoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1216, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1239, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1217, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1240, self, varargin{:});
     end
     function self = ContactWrench(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -18,14 +18,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1218, varargin{:});
+        tmp = iDynTreeMEX(1241, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1219, self);
+        iDynTreeMEX(1242, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Cylinder.m b/bindings/matlab/autogenerated/+iDynTree/Cylinder.m
index 552b36a8b6a..e8b26d1a381 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Cylinder.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Cylinder.m
@@ -2,31 +2,31 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1096, self);
+        iDynTreeMEX(1119, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1097, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1120, self, varargin{:});
     end
     function varargout = length(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1098, self);
+        varargout{1} = iDynTreeMEX(1121, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1099, self, varargin{1});
+        iDynTreeMEX(1122, self, varargin{1});
       end
     end
     function varargout = radius(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1100, self);
+        varargout{1} = iDynTreeMEX(1123, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1101, self, varargin{1});
+        iDynTreeMEX(1124, self, varargin{1});
       end
     end
     function self = Cylinder(varargin)
@@ -36,7 +36,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1102, varargin{:});
+        tmp = iDynTreeMEX(1125, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m
index 04c8e35a02b..539e203eb47 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1170, varargin{:});
+        tmp = iDynTreeMEX(1193, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1171, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1194, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1172, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1195, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1173, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1196, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1174, self);
+        iDynTreeMEX(1197, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m
index 754c3176527..d0e1a7055f0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1175, varargin{:});
+        tmp = iDynTreeMEX(1198, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1176, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1199, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1177, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1200, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1178, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1201, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1179, self);
+        iDynTreeMEX(1202, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m
index e68afe63d43..b3d5b249d7f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(824);
+    varargout{1} = iDynTreeMEX(847);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(825,varargin{1});
+    iDynTreeMEX(848,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m
index fd96da658a0..ecc01cf9c48 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(826);
+    varargout{1} = iDynTreeMEX(849);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(827,varargin{1});
+    iDynTreeMEX(850,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m b/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
new file mode 100644
index 00000000000..0793cfb0e42
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
@@ -0,0 +1,81 @@
+classdef DiscreteExtendedKalmanFilterHelper < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function varargout = ekf_f(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1671, self, varargin{:});
+    end
+    function varargout = ekf_h(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1672, self, varargin{:});
+    end
+    function varargout = ekfComputeJacobianF(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1673, self, varargin{:});
+    end
+    function varargout = ekfComputeJacobianH(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1674, self, varargin{:});
+    end
+    function varargout = ekfPredict(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1675, self, varargin{:});
+    end
+    function varargout = ekfUpdate(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1676, self, varargin{:});
+    end
+    function varargout = ekfInit(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1677, self, varargin{:});
+    end
+    function varargout = ekfReset(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1678, self, varargin{:});
+    end
+    function varargout = ekfSetMeasurementVector(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1679, self, varargin{:});
+    end
+    function varargout = ekfSetInputVector(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1680, self, varargin{:});
+    end
+    function varargout = ekfSetInitialState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1681, self, varargin{:});
+    end
+    function varargout = ekfSetStateCovariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1682, self, varargin{:});
+    end
+    function varargout = ekfSetSystemNoiseCovariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1683, self, varargin{:});
+    end
+    function varargout = ekfSetMeasurementNoiseCovariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1684, self, varargin{:});
+    end
+    function varargout = ekfSetStateSize(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1685, self, varargin{:});
+    end
+    function varargout = ekfSetInputSize(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1686, self, varargin{:});
+    end
+    function varargout = ekfSetOutputSize(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1687, self, varargin{:});
+    end
+    function varargout = ekfGetStates(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1688, self, varargin{:});
+    end
+    function varargout = ekfGetStateCovariance(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1689, self, varargin{:});
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1690, self);
+        self.SwigClear();
+      end
+    end
+    function self = DiscreteExtendedKalmanFilterHelper(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        error('No matching constructor');
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m b/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m
new file mode 100644
index 00000000000..f6ae3cdc1bf
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m
@@ -0,0 +1,86 @@
+classdef DynamicSpan < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function self = DynamicSpan(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(818, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(819, self);
+        self.SwigClear();
+      end
+    end
+    function varargout = first(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(820, self, varargin{:});
+    end
+    function varargout = last(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(821, self, varargin{:});
+    end
+    function varargout = subspan(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(822, self, varargin{:});
+    end
+    function varargout = size(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(823, self, varargin{:});
+    end
+    function varargout = size_bytes(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(824, self, varargin{:});
+    end
+    function varargout = empty(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(825, self, varargin{:});
+    end
+    function varargout = brace(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(826, self, varargin{:});
+    end
+    function varargout = getVal(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(827, self, varargin{:});
+    end
+    function varargout = setVal(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(828, self, varargin{:});
+    end
+    function varargout = at(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(829, self, varargin{:});
+    end
+    function varargout = paren(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(830, self, varargin{:});
+    end
+    function varargout = begin(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(831, self, varargin{:});
+    end
+    function varargout = end(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(832, self, varargin{:});
+    end
+    function varargout = cbegin(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(833, self, varargin{:});
+    end
+    function varargout = cend(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(834, self, varargin{:});
+    end
+    function varargout = rbegin(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(835, self, varargin{:});
+    end
+    function varargout = rend(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(836, self, varargin{:});
+    end
+    function varargout = crbegin(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(837, self, varargin{:});
+    end
+    function varargout = crend(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(838, self, varargin{:});
+    end
+  end
+  methods(Static)
+    function v = extent()
+      v = iDynTreeMEX(817);
+    end
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
index 5ea770c5ad4..1b1f4eb718d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
@@ -9,118 +9,118 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1788, varargin{:});
+        tmp = iDynTreeMEX(1945, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1789, self);
+        iDynTreeMEX(1946, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1790, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1947, self, varargin{:});
     end
     function varargout = loadRobotModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1791, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1948, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1792, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1949, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1793, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1950, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1794, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1951, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1795, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1952, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1796, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1953, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1797, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1954, self, varargin{:});
     end
     function varargout = getFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1798, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1955, self, varargin{:});
     end
     function varargout = setFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1799, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1956, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1800, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1957, self, varargin{:});
     end
     function varargout = getWorldBaseTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1801, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1958, self, varargin{:});
     end
     function varargout = getBaseTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1802, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1959, self, varargin{:});
     end
     function varargout = getJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1803, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1960, self, varargin{:});
     end
     function varargout = getJointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1804, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1961, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1805, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1962, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1806, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1963, self, varargin{:});
     end
     function varargout = getWorldTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1807, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1964, self, varargin{:});
     end
     function varargout = getRelativeTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1808, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1965, self, varargin{:});
     end
     function varargout = getFrameTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1809, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1966, self, varargin{:});
     end
     function varargout = getFrameTwistInWorldOrient(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1810, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1967, self, varargin{:});
     end
     function varargout = getFrameProperSpatialAcceleration(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1811, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1968, self, varargin{:});
     end
     function varargout = getLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1812, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1969, self, varargin{:});
     end
     function varargout = getLinkInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1813, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1970, self, varargin{:});
     end
     function varargout = getJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1814, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1971, self, varargin{:});
     end
     function varargout = getJointName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1815, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1972, self, varargin{:});
     end
     function varargout = getJointLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1816, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1973, self, varargin{:});
     end
     function varargout = inverseDynamics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1817, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1974, self, varargin{:});
     end
     function varargout = getFreeFloatingMassMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1818, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1975, self, varargin{:});
     end
     function varargout = getFrameJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1819, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1976, self, varargin{:});
     end
     function varargout = getDynamicsRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1820, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1977, self, varargin{:});
     end
     function varargout = getModelDynamicsParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1821, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1978, self, varargin{:});
     end
     function varargout = getCenterOfMass(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1822, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1979, self, varargin{:});
     end
     function varargout = getCenterOfMassJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1823, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1980, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
index 03bb98a415c..15d69d9051b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
@@ -9,100 +9,100 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1624, varargin{:});
+        tmp = iDynTreeMEX(1766, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1625, self);
+        iDynTreeMEX(1767, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotAndSensorsModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1626, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1768, self, varargin{:});
     end
     function varargout = loadRobotAndSensorsModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1627, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1769, self, varargin{:});
     end
     function varargout = loadRegressorStructureFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1628, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1770, self, varargin{:});
     end
     function varargout = loadRegressorStructureFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1629, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1771, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1630, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1772, self, varargin{:});
     end
     function varargout = getNrOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1631, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1773, self, varargin{:});
     end
     function varargout = getNrOfOutputs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1632, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1774, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1633, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1775, self, varargin{:});
     end
     function varargout = getDescriptionOfParameter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1634, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1776, self, varargin{:});
     end
     function varargout = getDescriptionOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1635, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1777, self, varargin{:});
     end
     function varargout = getDescriptionOfOutput(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1636, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1778, self, varargin{:});
     end
     function varargout = getDescriptionOfOutputs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1637, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1779, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1638, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1780, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1639, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1781, self, varargin{:});
     end
     function varargout = getDescriptionOfLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1640, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1782, self, varargin{:});
     end
     function varargout = getDescriptionOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1641, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1783, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1642, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1784, self, varargin{:});
     end
     function varargout = getNrOfFakeLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1643, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1785, self, varargin{:});
     end
     function varargout = getBaseLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1644, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1786, self, varargin{:});
     end
     function varargout = getSensorsModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1645, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1787, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1646, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1788, self, varargin{:});
     end
     function varargout = getSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1647, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1789, self, varargin{:});
     end
     function varargout = setTorqueSensorMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1648, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1790, self, varargin{:});
     end
     function varargout = computeRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1649, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1791, self, varargin{:});
     end
     function varargout = getModelParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1650, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1792, self, varargin{:});
     end
     function varargout = computeFloatingBaseIdentifiableSubspace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1651, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1793, self, varargin{:});
     end
     function varargout = computeFixedBaseIdentifiableSubspace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1652, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1794, self, varargin{:});
     end
     function varargout = generate_random_regressors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1653, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1795, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
index 8c2b1947388..7eb0344f8de 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1604, self);
+        varargout{1} = iDynTreeMEX(1746, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1605, self, varargin{1});
+        iDynTreeMEX(1747, self, varargin{1});
       end
     end
     function varargout = elemIndex(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1606, self);
+        varargout{1} = iDynTreeMEX(1748, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1607, self, varargin{1});
+        iDynTreeMEX(1749, self, varargin{1});
       end
     end
     function varargout = type(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1608, self);
+        varargout{1} = iDynTreeMEX(1750, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1609, self, varargin{1});
+        iDynTreeMEX(1751, self, varargin{1});
       end
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1610, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1752, self, varargin{:});
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1611, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1753, self, varargin{:});
     end
     function varargout = ne(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1612, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1754, self, varargin{:});
     end
     function self = DynamicsRegressorParameter(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -48,14 +48,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1613, varargin{:});
+        tmp = iDynTreeMEX(1755, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1614, self);
+        iDynTreeMEX(1756, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
index 0c65a8e0abf..8e166979883 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
@@ -7,26 +7,26 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1615, self);
+        varargout{1} = iDynTreeMEX(1757, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1616, self, varargin{1});
+        iDynTreeMEX(1758, self, varargin{1});
       end
     end
     function varargout = getDescriptionOfParameter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1617, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1759, self, varargin{:});
     end
     function varargout = addParam(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1618, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1760, self, varargin{:});
     end
     function varargout = addList(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1619, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1761, self, varargin{:});
     end
     function varargout = findParam(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1620, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1762, self, varargin{:});
     end
     function varargout = getNrOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1621, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1763, self, varargin{:});
     end
     function self = DynamicsRegressorParametersList(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -34,14 +34,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1622, varargin{:});
+        tmp = iDynTreeMEX(1764, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1623, self);
+        iDynTreeMEX(1765, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m b/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
index 4e27030b6d5..12eccf7781c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
@@ -9,52 +9,52 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1489, varargin{:});
+        tmp = iDynTreeMEX(1513, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1490, self);
+        iDynTreeMEX(1514, self);
         self.SwigClear();
       end
     end
     function varargout = setModelAndSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1491, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1515, self, varargin{:});
     end
     function varargout = loadModelAndSensorsFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1492, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1516, self, varargin{:});
     end
     function varargout = loadModelAndSensorsFromFileWithSpecifiedDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1493, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1517, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1494, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1518, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1495, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1519, self, varargin{:});
     end
     function varargout = submodels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1496, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1520, self, varargin{:});
     end
     function varargout = updateKinematicsFromFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1497, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1521, self, varargin{:});
     end
     function varargout = updateKinematicsFromFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1498, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1522, self, varargin{:});
     end
     function varargout = computeExpectedFTSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1499, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1523, self, varargin{:});
     end
     function varargout = estimateExtWrenchesAndJointTorques(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1500, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1524, self, varargin{:});
     end
     function varargout = checkThatTheModelIsStill(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1501, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1525, self, varargin{:});
     end
     function varargout = estimateLinkNetWrenchesWithoutGravity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1502, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1526, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m b/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m
index b358e3a8c01..dfc704e1ee7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m
@@ -2,31 +2,31 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1103, self);
+        iDynTreeMEX(1126, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1104, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1127, self, varargin{:});
     end
     function varargout = filename(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1105, self);
+        varargout{1} = iDynTreeMEX(1128, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1106, self, varargin{1});
+        iDynTreeMEX(1129, self, varargin{1});
       end
     end
     function varargout = scale(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1107, self);
+        varargout{1} = iDynTreeMEX(1130, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1108, self, varargin{1});
+        iDynTreeMEX(1131, self, varargin{1});
       end
     end
     function self = ExternalMesh(varargin)
@@ -36,7 +36,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1109, varargin{:});
+        tmp = iDynTreeMEX(1132, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m
index 617f3d2c4fe..152663177d0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(828);
+    varargout{1} = iDynTreeMEX(851);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(829,varargin{1});
+    iDynTreeMEX(852,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m
index 6bc2b86e4fa..9e3c4d061af 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(830);
+    varargout{1} = iDynTreeMEX(853);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(831,varargin{1});
+    iDynTreeMEX(854,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m b/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m
index 7125c099f8d..3fa0dbeecb0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m
@@ -7,103 +7,103 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(914, varargin{:});
+        tmp = iDynTreeMEX(937, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(915, self);
+        iDynTreeMEX(938, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(916, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(939, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(917, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(940, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(918, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(941, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(919, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(942, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(920, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(943, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(921, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(944, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(922, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(945, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(923, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(946, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(924, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(947, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(925, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(948, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(926, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(949, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(927, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(950, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(928, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(951, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(929, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(952, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(930, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(953, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(931, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(954, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(932, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(955, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(933, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(956, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(934, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(957, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(935, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(958, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(936, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(959, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(937, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(960, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(938, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(961, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(939, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(962, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(940, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(963, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(941, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(964, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(942, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(965, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(943, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(966, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(944, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(967, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m
index 89db5cfff11..274aed97076 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1233, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1256, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m
index 4b7899c1ea9..0d00f00821d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardBiasAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1234, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1257, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m
index dbac75eaf32..4b315d47c82 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardPosVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1231, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1254, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m
index b98b4230c9f..941959cc169 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardPosVelKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1232, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1255, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m
index 6ed9e541e21..a617c6a2bb3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardPositionKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1229, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1252, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m
index 809f076dcb7..81d3bb3a2ef 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1230, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1253, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m b/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m
index 4ce0a353456..f66efc827bc 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1180, varargin{:});
+        tmp = iDynTreeMEX(1203, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1181, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1204, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1182, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1205, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1183, self);
+        iDynTreeMEX(1206, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m
index 231f77f9fcf..60eb8fce85e 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1209, varargin{:});
+        tmp = iDynTreeMEX(1232, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1210, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1233, self, varargin{:});
     end
     function varargout = baseAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1211, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1234, self, varargin{:});
     end
     function varargout = jointAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1212, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1235, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1213, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1236, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1214, self);
+        iDynTreeMEX(1237, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m
index 53a67a4f493..2505d7f8dc0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1197, varargin{:});
+        tmp = iDynTreeMEX(1220, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1198, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1221, self, varargin{:});
     end
     function varargout = baseWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1199, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1222, self, varargin{:});
     end
     function varargout = jointTorques(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1200, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1223, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1201, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1224, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1202, self);
+        iDynTreeMEX(1225, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m
index 2f0bb93408f..84ed283777b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m
@@ -7,17 +7,17 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1188, varargin{:});
+        tmp = iDynTreeMEX(1211, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1189, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1212, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1190, self);
+        iDynTreeMEX(1213, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m
index f2f95934dd5..53585e6b414 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1191, varargin{:});
+        tmp = iDynTreeMEX(1214, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1192, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1215, self, varargin{:});
     end
     function varargout = worldBasePos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1193, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1216, self, varargin{:});
     end
     function varargout = jointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1194, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1217, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1195, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1218, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1196, self);
+        iDynTreeMEX(1219, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m
index 0a6f84534a8..273849c0cf4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1203, varargin{:});
+        tmp = iDynTreeMEX(1226, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1204, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1227, self, varargin{:});
     end
     function varargout = baseVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1205, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1228, self, varargin{:});
     end
     function varargout = jointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1206, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1229, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1207, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1230, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1208, self);
+        iDynTreeMEX(1231, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m b/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m
index 30985f57be6..13bb760b091 100644
--- a/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m
@@ -7,58 +7,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1361, varargin{:});
+        tmp = iDynTreeMEX(1385, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1362, self);
+        iDynTreeMEX(1386, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1363, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1387, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1364, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1388, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1365, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1389, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1366, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1390, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1367, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1391, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1368, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1392, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1369, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1393, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1370, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1394, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1371, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1395, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1372, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1396, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1373, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1397, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1374, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1398, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1375, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1399, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1376, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1400, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m b/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
new file mode 100644
index 00000000000..d0334cbb391
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
@@ -0,0 +1,54 @@
+classdef IAttitudeEstimator < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1628, self);
+        self.SwigClear();
+      end
+    end
+    function varargout = updateFilterWithMeasurements(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1629, self, varargin{:});
+    end
+    function varargout = propagateStates(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1630, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1631, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsQuaternion(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1632, self, varargin{:});
+    end
+    function varargout = getOrientationEstimateAsRPY(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1633, self, varargin{:});
+    end
+    function varargout = getInternalStateSize(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1634, self, varargin{:});
+    end
+    function varargout = getInternalState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1635, self, varargin{:});
+    end
+    function varargout = getDefaultInternalInitialState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1636, self, varargin{:});
+    end
+    function varargout = setInternalState(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1637, self, varargin{:});
+    end
+    function varargout = setInternalStateInitialOrientation(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1638, self, varargin{:});
+    end
+    function self = IAttitudeEstimator(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        error('No matching constructor');
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ICamera.m b/bindings/matlab/autogenerated/+iDynTree/ICamera.m
index 1625fcc72bc..5632ce6d9cc 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ICamera.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ICamera.m
@@ -5,18 +5,18 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1706, self);
+        iDynTreeMEX(1851, self);
         self.SwigClear();
       end
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1707, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1852, self, varargin{:});
     end
     function varargout = setTarget(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1708, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1853, self, varargin{:});
     end
     function varargout = setUpVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1709, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1854, self, varargin{:});
     end
     function self = ICamera(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m b/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
index 1bf1dab327b..56e883a12eb 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
@@ -5,33 +5,33 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1734, self);
+        iDynTreeMEX(1879, self);
         self.SwigClear();
       end
     end
     function varargout = getElements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1735, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1880, self, varargin{:});
     end
     function varargout = setElementVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1736, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1881, self, varargin{:});
     end
     function varargout = setBackgroundColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1737, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1882, self, varargin{:});
     end
     function varargout = setAmbientLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1738, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1883, self, varargin{:});
     end
     function varargout = getLights(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1739, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1884, self, varargin{:});
     end
     function varargout = addLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1740, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1885, self, varargin{:});
     end
     function varargout = lightViz(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1741, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1886, self, varargin{:});
     end
     function varargout = removeLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1742, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1887, self, varargin{:});
     end
     function self = IEnvironment(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
index 7f2e9b93752..f3ffecfc2a6 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
@@ -5,30 +5,30 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1743, self);
+        iDynTreeMEX(1888, self);
         self.SwigClear();
       end
     end
     function varargout = setJetsFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1744, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1889, self, varargin{:});
     end
     function varargout = getNrOfJets(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1745, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1890, self, varargin{:});
     end
     function varargout = getJetDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1746, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1891, self, varargin{:});
     end
     function varargout = setJetDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1747, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1892, self, varargin{:});
     end
     function varargout = setJetColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1748, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1893, self, varargin{:});
     end
     function varargout = setJetsDimensions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1749, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1894, self, varargin{:});
     end
     function varargout = setJetsIntensity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1750, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1895, self, varargin{:});
     end
     function self = IJetsVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IJoint.m b/bindings/matlab/autogenerated/+iDynTree/IJoint.m
index dab0f1fbdb2..c1135c53484 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IJoint.m
@@ -5,108 +5,108 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(880, self);
+        iDynTreeMEX(903, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(881, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(904, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(882, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(905, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(883, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(906, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(884, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(907, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(885, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(908, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(886, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(909, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(887, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(910, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(888, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(911, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(889, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(912, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(890, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(913, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(891, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(914, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(892, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(915, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(893, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(916, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(894, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(917, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(895, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(918, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(896, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(919, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(897, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(920, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(898, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(921, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(899, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(922, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(900, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(923, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(901, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(924, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(902, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(925, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(903, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(926, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(904, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(927, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(905, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(928, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(906, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(929, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(907, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(930, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(908, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(931, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(909, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(932, self, varargin{:});
     end
     function varargout = isRevoluteJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(910, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(933, self, varargin{:});
     end
     function varargout = isFixedJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(911, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(934, self, varargin{:});
     end
     function varargout = asRevoluteJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(912, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(935, self, varargin{:});
     end
     function varargout = asFixedJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(913, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(936, self, varargin{:});
     end
     function self = IJoint(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/ILight.m b/bindings/matlab/autogenerated/+iDynTree/ILight.m
index 408c90c6a2c..e3d8fd2bb51 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ILight.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ILight.m
@@ -5,48 +5,48 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1720, self);
+        iDynTreeMEX(1865, self);
         self.SwigClear();
       end
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1721, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1866, self, varargin{:});
     end
     function varargout = setType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1722, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1867, self, varargin{:});
     end
     function varargout = getType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1723, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1868, self, varargin{:});
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1724, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1869, self, varargin{:});
     end
     function varargout = getPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1725, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1870, self, varargin{:});
     end
     function varargout = setDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1726, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1871, self, varargin{:});
     end
     function varargout = getDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1727, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1872, self, varargin{:});
     end
     function varargout = setAmbientColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1728, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1873, self, varargin{:});
     end
     function varargout = getAmbientColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1729, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1874, self, varargin{:});
     end
     function varargout = setSpecularColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1730, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1875, self, varargin{:});
     end
     function varargout = getSpecularColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1731, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1876, self, varargin{:});
     end
     function varargout = setDiffuseColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1732, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1877, self, varargin{:});
     end
     function varargout = getDiffuseColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1733, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1878, self, varargin{:});
     end
     function self = ILight(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
index 14dba65ace7..4763aa4d64b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
@@ -5,45 +5,57 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1751, self);
+        iDynTreeMEX(1903, self);
         self.SwigClear();
       end
     end
     function varargout = setPositions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1752, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1904, self, varargin{:});
     end
     function varargout = setLinkPositions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1753, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1905, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1754, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1906, self, varargin{:});
     end
     function varargout = getInstanceName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1755, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1907, self, varargin{:});
     end
     function varargout = setModelVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1756, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1908, self, varargin{:});
     end
     function varargout = setModelColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1757, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1909, self, varargin{:});
     end
     function varargout = resetModelColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1758, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1910, self, varargin{:});
+    end
+    function varargout = setLinkColor(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1911, self, varargin{:});
+    end
+    function varargout = resetLinkColor(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1912, self, varargin{:});
     end
     function varargout = getLinkNames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1759, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1913, self, varargin{:});
     end
     function varargout = setLinkVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1760, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1914, self, varargin{:});
     end
     function varargout = getFeatures(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1761, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1915, self, varargin{:});
     end
     function varargout = setFeatureVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1762, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1916, self, varargin{:});
     end
     function varargout = jets(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1763, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1917, self, varargin{:});
+    end
+    function varargout = getWorldModelTransform(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1918, self, varargin{:});
+    end
+    function varargout = getWorldLinkTransform(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1919, self, varargin{:});
     end
     function self = IModelVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
new file mode 100644
index 00000000000..9660f6a0265
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
@@ -0,0 +1,42 @@
+classdef IVectorsVisualization < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1896, self);
+        self.SwigClear();
+      end
+    end
+    function varargout = addVector(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1897, self, varargin{:});
+    end
+    function varargout = getNrOfVectors(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1898, self, varargin{:});
+    end
+    function varargout = getVector(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1899, self, varargin{:});
+    end
+    function varargout = updateVector(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1900, self, varargin{:});
+    end
+    function varargout = setVectorColor(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1901, self, varargin{:});
+    end
+    function varargout = setVectorsAspect(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1902, self, varargin{:});
+    end
+    function self = IVectorsVisualization(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        error('No matching constructor');
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m b/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m
new file mode 100644
index 00000000000..e22317fcdc1
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m
@@ -0,0 +1,3 @@
+function varargout = InverseDynamicsInertialParametersRegressor(varargin)
+  [varargout{1:nargout}] = iDynTreeMEX(1285, varargin{:});
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m
index 7e8a380ec96..7184dc2f826 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(820);
+    varargout{1} = iDynTreeMEX(843);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(821,varargin{1});
+    iDynTreeMEX(844,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m
index b8a7f2a66d1..bfe7bf00125 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(822);
+    varargout{1} = iDynTreeMEX(845);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(823,varargin{1});
+    iDynTreeMEX(846,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m b/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m
index d4fe3eb5bb7..dfaf39752d7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1166, varargin{:});
+        tmp = iDynTreeMEX(1189, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1167, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1190, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1168, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1191, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1169, self);
+        iDynTreeMEX(1192, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m b/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m
index 5c77608fb9d..f057d806b29 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1162, varargin{:});
+        tmp = iDynTreeMEX(1185, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1163, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1186, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1164, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1187, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1165, self);
+        iDynTreeMEX(1188, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JointSensor.m b/bindings/matlab/autogenerated/+iDynTree/JointSensor.m
index 58598c7b9e4..beed170942a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JointSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JointSensor.m
@@ -2,24 +2,24 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1275, self);
+        iDynTreeMEX(1299, self);
         self.SwigClear();
       end
     end
     function varargout = getParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1276, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1300, self, varargin{:});
     end
     function varargout = getParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1277, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1301, self, varargin{:});
     end
     function varargout = setParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1278, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1302, self, varargin{:});
     end
     function varargout = setParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1279, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1303, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1280, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1304, self, varargin{:});
     end
     function self = JointSensor(varargin)
       self@iDynTree.Sensor(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m b/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
index 4358402fe14..ef8e60ac8a1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
+++ b/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
@@ -9,166 +9,175 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1654, varargin{:});
+        tmp = iDynTreeMEX(1796, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1655, self);
+        iDynTreeMEX(1797, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1656, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1798, self, varargin{:});
     end
     function varargout = loadRobotModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1657, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1799, self, varargin{:});
     end
     function varargout = loadRobotModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1658, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1800, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1659, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1801, self, varargin{:});
     end
     function varargout = setFrameVelocityRepresentation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1660, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1802, self, varargin{:});
     end
     function varargout = getFrameVelocityRepresentation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1661, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1803, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1662, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1804, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1663, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1805, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1664, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1806, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1665, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1807, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1666, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1808, self, varargin{:});
     end
     function varargout = getFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1667, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1809, self, varargin{:});
     end
     function varargout = setFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1668, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1810, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1669, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1811, self, varargin{:});
     end
     function varargout = getRobotModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1670, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1812, self, varargin{:});
     end
     function varargout = getRelativeJacobianSparsityPattern(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1671, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1813, self, varargin{:});
     end
     function varargout = getFrameFreeFloatingJacobianSparsityPattern(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1672, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1814, self, varargin{:});
     end
     function varargout = setJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1673, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1815, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1674, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1816, self, varargin{:});
     end
     function varargout = getRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1675, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1817, self, varargin{:});
     end
     function varargout = getWorldBaseTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1676, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1818, self, varargin{:});
     end
     function varargout = getBaseTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1677, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1819, self, varargin{:});
     end
     function varargout = getJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1678, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1820, self, varargin{:});
     end
     function varargout = getJointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1679, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1821, self, varargin{:});
     end
     function varargout = getModelVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1680, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1822, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1681, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1823, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1682, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1824, self, varargin{:});
     end
     function varargout = getWorldTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1683, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1825, self, varargin{:});
     end
     function varargout = getRelativeTransformExplicit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1684, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1826, self, varargin{:});
     end
     function varargout = getRelativeTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1685, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1827, self, varargin{:});
     end
     function varargout = getFrameVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1686, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1828, self, varargin{:});
+    end
+    function varargout = getFrameAcc(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1829, self, varargin{:});
     end
     function varargout = getFrameFreeFloatingJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1687, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1830, self, varargin{:});
     end
     function varargout = getRelativeJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1688, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1831, self, varargin{:});
     end
     function varargout = getRelativeJacobianExplicit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1689, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1832, self, varargin{:});
     end
     function varargout = getFrameBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1690, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1833, self, varargin{:});
     end
     function varargout = getCenterOfMassPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1691, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1834, self, varargin{:});
     end
     function varargout = getCenterOfMassVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1692, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1835, self, varargin{:});
     end
     function varargout = getCenterOfMassJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1693, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1836, self, varargin{:});
     end
     function varargout = getCenterOfMassBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1694, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1837, self, varargin{:});
     end
     function varargout = getAverageVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1695, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1838, self, varargin{:});
     end
     function varargout = getAverageVelocityJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1696, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1839, self, varargin{:});
     end
     function varargout = getCentroidalAverageVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1697, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1840, self, varargin{:});
     end
     function varargout = getCentroidalAverageVelocityJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1698, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1841, self, varargin{:});
     end
     function varargout = getLinearAngularMomentum(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1699, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1842, self, varargin{:});
     end
     function varargout = getLinearAngularMomentumJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1700, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1843, self, varargin{:});
     end
     function varargout = getCentroidalTotalMomentum(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1701, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1844, self, varargin{:});
     end
     function varargout = getFreeFloatingMassMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1702, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1845, self, varargin{:});
     end
     function varargout = inverseDynamics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1703, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1846, self, varargin{:});
     end
     function varargout = generalizedBiasForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1704, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1847, self, varargin{:});
     end
     function varargout = generalizedGravityForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1705, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1848, self, varargin{:});
+    end
+    function varargout = generalizedExternalForces(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1849, self, varargin{:});
+    end
+    function varargout = inverseDynamicsInertialParametersRegressor(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1850, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m
index 6ea47d7f112..1e46c9a1719 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(816);
+    varargout{1} = iDynTreeMEX(839);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(817,varargin{1});
+    iDynTreeMEX(840,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m
index bc731921af2..498c1325753 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(818);
+    varargout{1} = iDynTreeMEX(841);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(819,varargin{1});
+    iDynTreeMEX(842,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Link.m b/bindings/matlab/autogenerated/+iDynTree/Link.m
index 31073a0d60f..839cc5ff365 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Link.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Link.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(873, varargin{:});
+        tmp = iDynTreeMEX(896, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = inertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(874, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(897, self, varargin{:});
     end
     function varargout = setInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(875, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(898, self, varargin{:});
     end
     function varargout = getInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(876, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(899, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(877, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(900, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(878, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(901, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(879, self);
+        iDynTreeMEX(902, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m b/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m
index ab0ba7587bc..9b9943aaf32 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(866, varargin{:});
+        tmp = iDynTreeMEX(889, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(867, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(890, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(868, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(891, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(869, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(892, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(870, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(893, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(871, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(894, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(872, self);
+        iDynTreeMEX(895, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m b/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m
index 4d6f67bef16..b6b3800b939 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(854, varargin{:});
+        tmp = iDynTreeMEX(877, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(855, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(878, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(856, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(879, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(857, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(880, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(858, self);
+        iDynTreeMEX(881, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m b/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m
index 0128ae4878d..e5fb0a81664 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m
@@ -9,35 +9,35 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1220, varargin{:});
+        tmp = iDynTreeMEX(1243, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1221, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1244, self, varargin{:});
     end
     function varargout = getNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1222, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1245, self, varargin{:});
     end
     function varargout = setNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1223, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1246, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1224, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1247, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1225, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1248, self, varargin{:});
     end
     function varargout = computeNetWrenches(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1226, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1249, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1227, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1250, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1228, self);
+        iDynTreeMEX(1251, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m b/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m
index ccd8145e762..9d57ff07a8b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(849, varargin{:});
+        tmp = iDynTreeMEX(872, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(850, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(873, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(851, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(874, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(852, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(875, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(853, self);
+        iDynTreeMEX(876, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m b/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m
index 8420dbb85c5..73977b8f48a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(834, varargin{:});
+        tmp = iDynTreeMEX(857, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(835, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(858, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(836, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(859, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(837, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(860, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(838, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(861, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(839, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(862, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(840, self);
+        iDynTreeMEX(863, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m b/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m
index 13c882741bc..dca85adaebc 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m
@@ -2,30 +2,30 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1281, self);
+        iDynTreeMEX(1305, self);
         self.SwigClear();
       end
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1282, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1306, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1283, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1307, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1284, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1308, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1285, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1309, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1286, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1310, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1287, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1311, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1288, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1312, self, varargin{:});
     end
     function self = LinkSensor(varargin)
       self@iDynTree.Sensor(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m b/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
index bf62682f04f..dddb7bdec43 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
@@ -9,41 +9,41 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1454, varargin{:});
+        tmp = iDynTreeMEX(1478, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = clear(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1455, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1479, self, varargin{:});
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1456, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1480, self, varargin{:});
     end
     function varargout = getNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1457, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1481, self, varargin{:});
     end
     function varargout = setNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1458, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1482, self, varargin{:});
     end
     function varargout = addNewContactForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1459, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1483, self, varargin{:});
     end
     function varargout = addNewContactInFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1460, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1484, self, varargin{:});
     end
     function varargout = addNewUnknownFullWrenchInFrameOrigin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1461, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1485, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1462, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1486, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1463, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1487, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1464, self);
+        iDynTreeMEX(1488, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m b/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m
index 44dee63fcf6..e42fb096166 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(859, varargin{:});
+        tmp = iDynTreeMEX(882, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(860, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(883, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(861, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(884, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(862, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(885, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(863, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(886, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(864, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(887, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(865, self);
+        iDynTreeMEX(888, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m b/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m
index 0892aecabae..99b05c49f47 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m
@@ -9,32 +9,32 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(841, varargin{:});
+        tmp = iDynTreeMEX(864, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(842, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(865, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(843, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(866, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(844, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(867, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(845, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(868, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(846, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(869, self, varargin{:});
     end
     function varargout = zero(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(847, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(870, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(848, self);
+        iDynTreeMEX(871, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Model.m b/bindings/matlab/autogenerated/+iDynTree/Model.m
index 15d10b47869..b1d338ea032 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Model.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Model.m
@@ -9,127 +9,127 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1123, varargin{:});
+        tmp = iDynTreeMEX(1146, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = copy(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1124, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1147, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1125, self);
+        iDynTreeMEX(1148, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1126, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1149, self, varargin{:});
     end
     function varargout = getLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1127, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1150, self, varargin{:});
     end
     function varargout = getLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1128, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1151, self, varargin{:});
     end
     function varargout = isValidLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1129, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1152, self, varargin{:});
     end
     function varargout = getLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1130, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1153, self, varargin{:});
     end
     function varargout = addLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1131, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1154, self, varargin{:});
     end
     function varargout = getNrOfJoints(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1132, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1155, self, varargin{:});
     end
     function varargout = getJointName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1133, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1156, self, varargin{:});
     end
     function varargout = getJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1134, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1157, self, varargin{:});
     end
     function varargout = getJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1135, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1158, self, varargin{:});
     end
     function varargout = isValidJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1136, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1159, self, varargin{:});
     end
     function varargout = isLinkNameUsed(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1137, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1160, self, varargin{:});
     end
     function varargout = isJointNameUsed(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1138, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1161, self, varargin{:});
     end
     function varargout = isFrameNameUsed(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1139, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1162, self, varargin{:});
     end
     function varargout = addJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1140, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1163, self, varargin{:});
     end
     function varargout = addJointAndLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1141, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1164, self, varargin{:});
     end
     function varargout = insertLinkToExistingJointAndAddJointForDisplacedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1142, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1165, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1143, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1166, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1144, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1167, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1145, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1168, self, varargin{:});
     end
     function varargout = addAdditionalFrameToLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1146, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1169, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1147, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1170, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1148, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1171, self, varargin{:});
     end
     function varargout = isValidFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1149, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1172, self, varargin{:});
     end
     function varargout = getFrameTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1150, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1173, self, varargin{:});
     end
     function varargout = getFrameLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1151, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1174, self, varargin{:});
     end
     function varargout = getNrOfNeighbors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1152, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1175, self, varargin{:});
     end
     function varargout = getNeighbor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1153, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1176, self, varargin{:});
     end
     function varargout = setDefaultBaseLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1154, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1177, self, varargin{:});
     end
     function varargout = getDefaultBaseLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1155, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1178, self, varargin{:});
     end
     function varargout = computeFullTreeTraversal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1156, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1179, self, varargin{:});
     end
     function varargout = getInertialParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1157, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1180, self, varargin{:});
     end
     function varargout = updateInertialParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1158, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1181, self, varargin{:});
     end
     function varargout = visualSolidShapes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1159, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1182, self, varargin{:});
     end
     function varargout = collisionSolidShapes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1160, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1183, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1161, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1184, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m b/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m
index cb2439638bd..fcce286db30 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m
@@ -9,46 +9,46 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1430, varargin{:});
+        tmp = iDynTreeMEX(1454, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1431, self);
+        iDynTreeMEX(1455, self);
         self.SwigClear();
       end
     end
     function varargout = parsingOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1432, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1456, self, varargin{:});
     end
     function varargout = setParsingOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1433, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1457, self, varargin{:});
     end
     function varargout = loadModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1434, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1458, self, varargin{:});
     end
     function varargout = loadModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1435, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1459, self, varargin{:});
     end
     function varargout = loadReducedModelFromFullModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1436, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1460, self, varargin{:});
     end
     function varargout = loadReducedModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1437, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1461, self, varargin{:});
     end
     function varargout = loadReducedModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1438, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1462, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1439, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1463, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1440, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1464, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1441, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1465, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m b/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m
index a60a36e30ab..f877b37c941 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m
@@ -7,20 +7,20 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1424, self);
+        varargout{1} = iDynTreeMEX(1448, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1425, self, varargin{1});
+        iDynTreeMEX(1449, self, varargin{1});
       end
     end
     function varargout = originalFilename(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1426, self);
+        varargout{1} = iDynTreeMEX(1450, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1427, self, varargin{1});
+        iDynTreeMEX(1451, self, varargin{1});
       end
     end
     function self = ModelParserOptions(varargin)
@@ -29,14 +29,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1428, varargin{:});
+        tmp = iDynTreeMEX(1452, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1429, self);
+        iDynTreeMEX(1453, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m b/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m
index 9b4da22859c..7b32de45721 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m
@@ -9,34 +9,34 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1110, varargin{:});
+        tmp = iDynTreeMEX(1133, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = clear(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1111, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1134, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1112, self);
+        iDynTreeMEX(1135, self);
         self.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1113, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1136, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1114, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1137, self, varargin{:});
     end
     function varargout = linkSolidShapes(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1115, self);
+        varargout{1} = iDynTreeMEX(1138, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1116, self, varargin{1});
+        iDynTreeMEX(1139, self, varargin{1});
       end
     end
   end
diff --git a/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m b/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m
index 47062c62808..167733c3ec1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1184, varargin{:});
+        tmp = iDynTreeMEX(1207, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1185, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1208, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1186, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1209, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1187, self);
+        iDynTreeMEX(1210, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m
index 2405c6832f7..c27613e7f90 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(945, self);
+        iDynTreeMEX(968, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(946, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(969, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(947, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(970, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(948, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(971, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(949, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(972, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(950, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(973, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(951, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(974, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(952, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(975, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(953, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(976, self, varargin{:});
     end
     function self = MovableJointImpl1(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m
index a4c31655d2f..17182a7d7e7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(954, self);
+        iDynTreeMEX(977, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(955, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(978, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(956, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(979, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(957, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(980, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(958, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(981, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(959, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(982, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(960, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(983, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(961, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(984, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(962, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(985, self, varargin{:});
     end
     function self = MovableJointImpl2(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m
index b71986ee070..07a8643c0f1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(963, self);
+        iDynTreeMEX(986, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(964, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(987, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(965, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(988, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(966, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(989, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(967, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(990, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(968, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(991, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(969, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(992, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(970, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(993, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(971, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(994, self, varargin{:});
     end
     function self = MovableJointImpl3(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m
index dadbd2ae768..d1f5829dcbe 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(972, self);
+        iDynTreeMEX(995, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(973, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(996, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(974, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(997, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(975, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(998, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(976, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(999, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(977, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1000, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(978, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1001, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(979, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1002, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(980, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1003, self, varargin{:});
     end
     function self = MovableJointImpl4(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m
index e0d50469012..ea277a67688 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(981, self);
+        iDynTreeMEX(1004, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(982, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1005, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(983, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1006, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(984, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1007, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(985, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1008, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(986, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1009, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(987, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1010, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(988, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1011, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(989, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1012, self, varargin{:});
     end
     function self = MovableJointImpl5(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m
index 9ebace56fb2..3c7e8f37c93 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(990, self);
+        iDynTreeMEX(1013, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(991, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1014, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(992, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1015, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(993, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1016, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(994, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1017, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(995, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1018, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(996, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1019, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(997, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1020, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(998, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1021, self, varargin{:});
     end
     function self = MovableJointImpl6(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m b/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m
index 519be756f30..4085e249c55 100644
--- a/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m
+++ b/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m
@@ -1,3 +1,3 @@
 function v = NR_OF_SENSOR_TYPES()
-  v = iDynTreeMEX(1262);
+  v = iDynTreeMEX(1286);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Neighbor.m b/bindings/matlab/autogenerated/+iDynTree/Neighbor.m
index fd61ac62e6a..2991e4f4982 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Neighbor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Neighbor.m
@@ -7,20 +7,20 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1117, self);
+        varargout{1} = iDynTreeMEX(1140, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1118, self, varargin{1});
+        iDynTreeMEX(1141, self, varargin{1});
       end
     end
     function varargout = neighborJoint(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1119, self);
+        varargout{1} = iDynTreeMEX(1142, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1120, self, varargin{1});
+        iDynTreeMEX(1143, self, varargin{1});
       end
     end
     function self = Neighbor(varargin)
@@ -29,14 +29,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1121, varargin{:});
+        tmp = iDynTreeMEX(1144, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1122, self);
+        iDynTreeMEX(1145, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m b/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m
index aa406563d75..cc05c34409f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m
@@ -7,85 +7,85 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1024, varargin{:});
+        tmp = iDynTreeMEX(1047, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1025, self);
+        iDynTreeMEX(1048, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1026, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1049, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1027, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1050, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1028, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1051, self, varargin{:});
     end
     function varargout = setAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1029, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1052, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1030, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1053, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1031, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1054, self, varargin{:});
     end
     function varargout = getAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1032, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1055, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1033, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1056, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1034, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1057, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1035, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1058, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1036, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1059, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1037, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1060, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1038, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1061, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1039, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1062, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1040, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1063, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1041, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1064, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1042, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1065, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1043, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1066, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1044, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1067, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1045, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1068, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1046, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1069, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1047, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1070, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1048, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1071, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m b/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m
index 6b4125c6390..72fb05fb9d4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m
+++ b/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m
@@ -1,3 +1,3 @@
 function varargout = RNEADynamicPhase(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1237, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1260, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m b/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m
index 226e51dfff1..39ed95cd3ac 100644
--- a/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m
@@ -7,85 +7,85 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(999, varargin{:});
+        tmp = iDynTreeMEX(1022, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1000, self);
+        iDynTreeMEX(1023, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1001, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1024, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1002, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1025, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1003, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1026, self, varargin{:});
     end
     function varargout = setAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1004, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1027, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1005, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1028, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1006, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1029, self, varargin{:});
     end
     function varargout = getAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1007, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1030, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1008, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1031, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1009, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1032, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1010, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1033, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1011, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1034, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1012, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1035, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1013, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1036, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1014, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1037, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1015, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1038, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1016, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1039, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1017, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1040, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1018, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1041, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1019, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1042, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1020, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1043, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1021, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1044, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1022, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1045, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1023, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1046, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/Sensor.m b/bindings/matlab/autogenerated/+iDynTree/Sensor.m
index 49d263b5de7..4a500232632 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Sensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Sensor.m
@@ -5,33 +5,33 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1266, self);
+        iDynTreeMEX(1290, self);
         self.SwigClear();
       end
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1267, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1291, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1268, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1292, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1269, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1293, self, varargin{:});
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1270, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1294, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1271, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1295, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1272, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1296, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1273, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1297, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1274, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1298, self, varargin{:});
     end
     function self = Sensor(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/SensorsList.m b/bindings/matlab/autogenerated/+iDynTree/SensorsList.m
index 47bf664b406..addfdc70612 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SensorsList.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SensorsList.m
@@ -9,61 +9,61 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1289, varargin{:});
+        tmp = iDynTreeMEX(1313, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1290, self);
+        iDynTreeMEX(1314, self);
         self.SwigClear();
       end
     end
     function varargout = addSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1291, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1315, self, varargin{:});
     end
     function varargout = setSerialization(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1292, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1316, self, varargin{:});
     end
     function varargout = getSerialization(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1293, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1317, self, varargin{:});
     end
     function varargout = getNrOfSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1294, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1318, self, varargin{:});
     end
     function varargout = getSensorIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1295, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1319, self, varargin{:});
     end
     function varargout = getSizeOfAllSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1296, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1320, self, varargin{:});
     end
     function varargout = getSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1297, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1321, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1298, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1322, self, varargin{:});
     end
     function varargout = removeSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1299, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1323, self, varargin{:});
     end
     function varargout = removeAllSensorsOfType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1300, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1324, self, varargin{:});
     end
     function varargout = getSixAxisForceTorqueSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1301, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1325, self, varargin{:});
     end
     function varargout = getAccelerometerSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1302, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1326, self, varargin{:});
     end
     function varargout = getGyroscopeSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1303, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1327, self, varargin{:});
     end
     function varargout = getThreeAxisAngularAccelerometerSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1304, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1328, self, varargin{:});
     end
     function varargout = getThreeAxisForceTorqueContactSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1305, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1329, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m b/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m
index b66cbbdb89d..609dfc1233a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m
@@ -9,37 +9,37 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1306, varargin{:});
+        tmp = iDynTreeMEX(1330, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1307, self);
+        iDynTreeMEX(1331, self);
         self.SwigClear();
       end
     end
     function varargout = setNrOfSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1308, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1332, self, varargin{:});
     end
     function varargout = getNrOfSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1309, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1333, self, varargin{:});
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1310, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1334, self, varargin{:});
     end
     function varargout = toVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1311, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1335, self, varargin{:});
     end
     function varargout = setMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1312, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1336, self, varargin{:});
     end
     function varargout = getMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1313, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1337, self, varargin{:});
     end
     function varargout = getSizeOfAllSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1314, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1338, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m b/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
index 3e3e1705b86..bb8d1653b26 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
@@ -9,43 +9,43 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1503, varargin{:});
+        tmp = iDynTreeMEX(1527, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1504, self);
+        iDynTreeMEX(1528, self);
         self.SwigClear();
       end
     end
     function varargout = setModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1505, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1529, self, varargin{:});
     end
     function varargout = loadModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1506, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1530, self, varargin{:});
     end
     function varargout = loadModelFromFileWithSpecifiedDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1507, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1531, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1508, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1532, self, varargin{:});
     end
     function varargout = updateKinematics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1509, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1533, self, varargin{:});
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1510, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1534, self, varargin{:});
     end
     function varargout = changeFixedFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1511, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1535, self, varargin{:});
     end
     function varargout = getCurrentFixedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1512, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1536, self, varargin{:});
     end
     function varargout = getWorldLinkTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1513, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1537, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m b/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m
index 75422efb51e..3bea68b5f6d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m
@@ -7,100 +7,100 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1315, varargin{:});
+        tmp = iDynTreeMEX(1339, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1316, self);
+        iDynTreeMEX(1340, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1317, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1341, self, varargin{:});
     end
     function varargout = setFirstLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1318, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1342, self, varargin{:});
     end
     function varargout = setSecondLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1319, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1343, self, varargin{:});
     end
     function varargout = getFirstLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1320, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1344, self, varargin{:});
     end
     function varargout = getSecondLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1321, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1345, self, varargin{:});
     end
     function varargout = setFirstLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1322, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1346, self, varargin{:});
     end
     function varargout = setSecondLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1323, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1347, self, varargin{:});
     end
     function varargout = getFirstLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1324, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1348, self, varargin{:});
     end
     function varargout = getSecondLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1325, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1349, self, varargin{:});
     end
     function varargout = setParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1326, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1350, self, varargin{:});
     end
     function varargout = setParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1327, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1351, self, varargin{:});
     end
     function varargout = setAppliedWrenchLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1328, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1352, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1329, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1353, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1330, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1354, self, varargin{:});
     end
     function varargout = getParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1331, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1355, self, varargin{:});
     end
     function varargout = getParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1332, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1356, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1333, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1357, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1334, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1358, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1335, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1359, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1336, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1360, self, varargin{:});
     end
     function varargout = getAppliedWrenchLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1337, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1361, self, varargin{:});
     end
     function varargout = isLinkAttachedToSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1338, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1362, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1339, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1363, self, varargin{:});
     end
     function varargout = getWrenchAppliedOnLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1340, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1364, self, varargin{:});
     end
     function varargout = getWrenchAppliedOnLinkMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1341, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1365, self, varargin{:});
     end
     function varargout = getWrenchAppliedOnLinkInverseMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1342, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1366, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1343, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1367, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1344, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1368, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SolidShape.m b/bindings/matlab/autogenerated/+iDynTree/SolidShape.m
index 82c8e3d9de7..6cbc10a7038 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SolidShape.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SolidShape.m
@@ -5,66 +5,66 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1066, self);
+        iDynTreeMEX(1089, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1067, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1090, self, varargin{:});
     end
     function varargout = name(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1068, self);
+        varargout{1} = iDynTreeMEX(1091, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1069, self, varargin{1});
+        iDynTreeMEX(1092, self, varargin{1});
       end
     end
     function varargout = link_H_geometry(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1070, self);
+        varargout{1} = iDynTreeMEX(1093, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1071, self, varargin{1});
+        iDynTreeMEX(1094, self, varargin{1});
       end
     end
     function varargout = material(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1072, self);
+        varargout{1} = iDynTreeMEX(1095, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1073, self, varargin{1});
+        iDynTreeMEX(1096, self, varargin{1});
       end
     end
     function varargout = isSphere(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1074, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1097, self, varargin{:});
     end
     function varargout = isBox(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1075, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1098, self, varargin{:});
     end
     function varargout = isCylinder(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1076, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1099, self, varargin{:});
     end
     function varargout = isExternalMesh(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1077, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1100, self, varargin{:});
     end
     function varargout = asSphere(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1078, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1101, self, varargin{:});
     end
     function varargout = asBox(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1079, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1102, self, varargin{:});
     end
     function varargout = asCylinder(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1080, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1103, self, varargin{:});
     end
     function varargout = asExternalMesh(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1081, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1104, self, varargin{:});
     end
     function self = SolidShape(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/Sphere.m b/bindings/matlab/autogenerated/+iDynTree/Sphere.m
index 94622eb0b37..15620360639 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Sphere.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Sphere.m
@@ -2,21 +2,21 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1082, self);
+        iDynTreeMEX(1105, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1083, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1106, self, varargin{:});
     end
     function varargout = radius(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1084, self);
+        varargout{1} = iDynTreeMEX(1107, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1085, self, varargin{1});
+        iDynTreeMEX(1108, self, varargin{1});
       end
     end
     function self = Sphere(varargin)
@@ -26,7 +26,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1086, varargin{:});
+        tmp = iDynTreeMEX(1109, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m
index 27bb8b797cb..643baa28a1a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(832);
+    varargout{1} = iDynTreeMEX(855);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(833,varargin{1});
+    iDynTreeMEX(856,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m
index 2a81b3f616d..957361e30d2 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m
@@ -7,55 +7,55 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1377, varargin{:});
+        tmp = iDynTreeMEX(1401, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1378, self);
+        iDynTreeMEX(1402, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1379, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1403, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1380, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1404, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1381, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1405, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1382, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1406, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1383, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1407, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1384, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1408, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1385, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1409, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1386, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1410, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1387, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1411, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1388, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1412, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1389, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1413, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1390, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1414, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1391, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1415, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m
index 6086ba35f0e..2fa14024d4f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m
@@ -7,64 +7,64 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1392, varargin{:});
+        tmp = iDynTreeMEX(1416, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1393, self);
+        iDynTreeMEX(1417, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1394, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1418, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1395, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1419, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1396, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1420, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1397, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1421, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1398, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1422, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1399, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1423, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1400, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1424, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1401, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1425, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1402, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1426, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1403, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1427, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1404, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1428, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1405, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1429, self, varargin{:});
     end
     function varargout = setLoadCellLocations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1406, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1430, self, varargin{:});
     end
     function varargout = getLoadCellLocations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1407, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1431, self, varargin{:});
     end
     function varargout = computeThreeAxisForceTorqueFromLoadCellMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1408, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1432, self, varargin{:});
     end
     function varargout = computeCenterOfPressureFromLoadCellMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1409, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1433, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/Traversal.m b/bindings/matlab/autogenerated/+iDynTree/Traversal.m
index 53181de3fa5..da1e002d234 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Traversal.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Traversal.m
@@ -9,61 +9,61 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1049, varargin{:});
+        tmp = iDynTreeMEX(1072, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1050, self);
+        iDynTreeMEX(1073, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfVisitedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1051, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1074, self, varargin{:});
     end
     function varargout = getLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1052, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1075, self, varargin{:});
     end
     function varargout = getBaseLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1053, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1076, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1054, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1077, self, varargin{:});
     end
     function varargout = getParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1055, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1078, self, varargin{:});
     end
     function varargout = getParentLinkFromLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1056, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1079, self, varargin{:});
     end
     function varargout = getParentJointFromLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1057, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1080, self, varargin{:});
     end
     function varargout = getTraversalIndexFromLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1058, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1081, self, varargin{:});
     end
     function varargout = reset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1059, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1082, self, varargin{:});
     end
     function varargout = addTraversalBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1060, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1083, self, varargin{:});
     end
     function varargout = addTraversalElement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1061, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1084, self, varargin{:});
     end
     function varargout = isParentOf(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1062, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1085, self, varargin{:});
     end
     function varargout = getChildLinkIndexFromJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1063, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1086, self, varargin{:});
     end
     function varargout = getParentLinkIndexFromJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1064, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1087, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1065, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1088, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m b/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m
index 6980aa7d167..bdb2428fd6d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m
@@ -7,20 +7,20 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1412, self);
+        varargout{1} = iDynTreeMEX(1436, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1413, self, varargin{1});
+        iDynTreeMEX(1437, self, varargin{1});
       end
     end
     function varargout = originalFilename(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1414, self);
+        varargout{1} = iDynTreeMEX(1438, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1415, self, varargin{1});
+        iDynTreeMEX(1439, self, varargin{1});
       end
     end
     function self = URDFParserOptions(varargin)
@@ -29,14 +29,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1416, varargin{:});
+        tmp = iDynTreeMEX(1440, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1417, self);
+        iDynTreeMEX(1441, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m b/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
index 82245024d20..2a98a8da0f0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
+++ b/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
@@ -9,7 +9,7 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1442, varargin{:});
+        tmp = iDynTreeMEX(1466, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
@@ -18,55 +18,55 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1443, self);
+        varargout{1} = iDynTreeMEX(1467, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1444, self, varargin{1});
+        iDynTreeMEX(1468, self, varargin{1});
       end
     end
     function varargout = contactPoint(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1445, self);
+        varargout{1} = iDynTreeMEX(1469, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1446, self, varargin{1});
+        iDynTreeMEX(1470, self, varargin{1});
       end
     end
     function varargout = forceDirection(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1447, self);
+        varargout{1} = iDynTreeMEX(1471, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1448, self, varargin{1});
+        iDynTreeMEX(1472, self, varargin{1});
       end
     end
     function varargout = knownWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1449, self);
+        varargout{1} = iDynTreeMEX(1473, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1450, self, varargin{1});
+        iDynTreeMEX(1474, self, varargin{1});
       end
     end
     function varargout = contactId(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1451, self);
+        varargout{1} = iDynTreeMEX(1475, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1452, self, varargin{1});
+        iDynTreeMEX(1476, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1453, self);
+        iDynTreeMEX(1477, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Visualizer.m b/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
index 86a3a9635f9..94117fe3ad6 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
@@ -9,52 +9,55 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1774, varargin{:});
+        tmp = iDynTreeMEX(1930, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1775, self);
+        iDynTreeMEX(1931, self);
         self.SwigClear();
       end
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1776, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1932, self, varargin{:});
     end
     function varargout = getNrOfVisualizedModels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1777, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1933, self, varargin{:});
     end
     function varargout = getModelInstanceName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1778, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1934, self, varargin{:});
     end
     function varargout = getModelInstanceIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1779, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1935, self, varargin{:});
     end
     function varargout = addModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1780, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1936, self, varargin{:});
     end
     function varargout = modelViz(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1781, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1937, self, varargin{:});
     end
     function varargout = camera(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1782, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1938, self, varargin{:});
     end
     function varargout = enviroment(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1783, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1939, self, varargin{:});
+    end
+    function varargout = vectors(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1940, self, varargin{:});
     end
     function varargout = run(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1784, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1941, self, varargin{:});
     end
     function varargout = draw(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1785, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1942, self, varargin{:});
     end
     function varargout = drawToFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1786, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1943, self, varargin{:});
     end
     function varargout = close(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1787, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1944, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m b/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
index ac5e23ff260..da9ac10c600 100644
--- a/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1764, self);
+        varargout{1} = iDynTreeMEX(1920, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1765, self, varargin{1});
+        iDynTreeMEX(1921, self, varargin{1});
       end
     end
     function varargout = winWidth(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1766, self);
+        varargout{1} = iDynTreeMEX(1922, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1767, self, varargin{1});
+        iDynTreeMEX(1923, self, varargin{1});
       end
     end
     function varargout = winHeight(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1768, self);
+        varargout{1} = iDynTreeMEX(1924, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1769, self, varargin{1});
+        iDynTreeMEX(1925, self, varargin{1});
       end
     end
     function varargout = rootFrameArrowsDimension(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1770, self);
+        varargout{1} = iDynTreeMEX(1926, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1771, self, varargin{1});
+        iDynTreeMEX(1927, self, varargin{1});
       end
     end
     function self = VisualizerOptions(varargin)
@@ -49,14 +49,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1772, varargin{:});
+        tmp = iDynTreeMEX(1928, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1773, self);
+        iDynTreeMEX(1929, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m b/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
index d16724b91d6..b63c5945733 100644
--- a/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
+++ b/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
@@ -1,3 +1,3 @@
 function varargout = computeLinkNetWrenchesWithoutGravity(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1487, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1511, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m
index ff531d9b9af..f96b7ef17c9 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m
@@ -1,3 +1,3 @@
 function varargout = dofsListFromURDF(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1420, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1444, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m
index b6595c36351..99c895327ab 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m
@@ -1,3 +1,3 @@
 function varargout = dofsListFromURDFString(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1421, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1445, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m b/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m
new file mode 100644
index 00000000000..34ff538ed5d
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m
@@ -0,0 +1,3 @@
+function v = dynamic_extent()
+  v = iDynTreeMEX(816);
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
index 5153107146e..d18bed729d4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = dynamicsEstimationForwardVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1485, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1509, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
index ffeaa718463..dde939d756e 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
@@ -1,3 +1,3 @@
 function varargout = dynamicsEstimationForwardVelKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1486, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1510, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
index e6c4645f0c9..96f63289218 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
@@ -1,3 +1,3 @@
 function varargout = estimateExternalWrenches(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1484, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1508, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
index 8469d852d10..90fc76b0daa 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
@@ -9,86 +9,86 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1465, varargin{:});
+        tmp = iDynTreeMEX(1489, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1466, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1490, self, varargin{:});
     end
     function varargout = getNrOfSubModels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1467, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1491, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1468, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1492, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1469, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1493, self, varargin{:});
     end
     function varargout = A(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1470, self);
+        varargout{1} = iDynTreeMEX(1494, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1471, self, varargin{1});
+        iDynTreeMEX(1495, self, varargin{1});
       end
     end
     function varargout = x(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1472, self);
+        varargout{1} = iDynTreeMEX(1496, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1473, self, varargin{1});
+        iDynTreeMEX(1497, self, varargin{1});
       end
     end
     function varargout = b(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1474, self);
+        varargout{1} = iDynTreeMEX(1498, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1475, self, varargin{1});
+        iDynTreeMEX(1499, self, varargin{1});
       end
     end
     function varargout = pinvA(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1476, self);
+        varargout{1} = iDynTreeMEX(1500, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1477, self, varargin{1});
+        iDynTreeMEX(1501, self, varargin{1});
       end
     end
     function varargout = b_contacts_subtree(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1478, self);
+        varargout{1} = iDynTreeMEX(1502, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1479, self, varargin{1});
+        iDynTreeMEX(1503, self, varargin{1});
       end
     end
     function varargout = subModelBase_H_link(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1480, self);
+        varargout{1} = iDynTreeMEX(1504, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1481, self, varargin{1});
+        iDynTreeMEX(1505, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1482, self);
+        iDynTreeMEX(1506, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
index d06ee1765fa..092ff059b6d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
@@ -1,3 +1,3 @@
 function varargout = estimateExternalWrenchesWithoutInternalFT(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1483, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1507, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m b/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
index 0d81af400ee..35d21940e16 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
@@ -1,3 +1,3 @@
 function varargout = estimateLinkContactWrenchesFromLinkNetExternalWrenches(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1488, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1512, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m b/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m
index fa5cf00322d..581f79578bf 100644
--- a/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m
+++ b/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m
@@ -1,3 +1,3 @@
 function varargout = getSensorTypeSize(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1265, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1289, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m b/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
new file mode 100644
index 00000000000..7254e435cc5
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
@@ -0,0 +1,3 @@
+function v = input_dimensions()
+  v = iDynTreeMEX(1693);
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
index bc4c2e3f494..a27b2a29046 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isDOFBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1516, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1540, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
index 50bb441a27d..e9f0fc52c67 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isJointBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1515, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1539, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m b/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m
index a81cc39cb0b..0cd34c1f019 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m
@@ -1,3 +1,3 @@
 function varargout = isJointSensor(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1264, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1288, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
index 0f89dfe14c1..5c74c2dc43b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isLinkBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1514, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1538, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m b/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m
index bfcce4d6b31..b3a289061f8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m
@@ -1,3 +1,3 @@
 function varargout = isLinkSensor(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1263, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1287, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m b/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m
index 55d51b0ee27..7057f74b286 100644
--- a/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m
@@ -1,3 +1,3 @@
 function varargout = modelFromURDF(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1418, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1442, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m b/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m
index 85c1860cae4..143d62b7213 100644
--- a/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m
+++ b/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m
@@ -1,3 +1,3 @@
 function varargout = modelFromURDFString(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1419, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1443, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
new file mode 100644
index 00000000000..c599cf3b597
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
@@ -0,0 +1,3 @@
+function v = output_dimensions_with_magnetometer()
+  v = iDynTreeMEX(1691);
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
new file mode 100644
index 00000000000..10a1e1f01a0
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
@@ -0,0 +1,3 @@
+function v = output_dimensions_without_magnetometer()
+  v = iDynTreeMEX(1692);
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m
index 48a5d7535b5..49b9e415caf 100644
--- a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m
+++ b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m
@@ -1,3 +1,3 @@
 function varargout = predictSensorsMeasurements(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1410, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1434, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m
index b5c0a927ed0..4f9a173b471 100644
--- a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m
@@ -1,3 +1,3 @@
 function varargout = predictSensorsMeasurementsFromRawBuffers(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1411, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1435, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m
index f372bc89fa0..8608cad5326 100644
--- a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m
@@ -1,3 +1,3 @@
 function varargout = sensorsFromURDF(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1422, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1446, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m
index 328b3a85a26..efb50b66751 100644
--- a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m
+++ b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m
@@ -1,3 +1,3 @@
 function varargout = sensorsFromURDFString(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1423, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1447, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
index 81cf7334fed..8850ccd97a5 100644
--- a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
+++ b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
@@ -1230,181 +1230,199 @@ namespace swig {
 #define SWIGTYPE_p_char swig_types[18]
 #define SWIGTYPE_p_const_iterator swig_types[19]
 #define SWIGTYPE_p_const_pointer swig_types[20]
-#define SWIGTYPE_p_const_typed_iterator swig_types[21]
-#define SWIGTYPE_p_difference_type swig_types[22]
-#define SWIGTYPE_p_double swig_types[23]
-#define SWIGTYPE_p_iDynTree__AccelerometerSensor swig_types[24]
-#define SWIGTYPE_p_iDynTree__AngularForceVector3 swig_types[25]
-#define SWIGTYPE_p_iDynTree__AngularForceVector3Semantics swig_types[26]
-#define SWIGTYPE_p_iDynTree__AngularMotionVector3 swig_types[27]
-#define SWIGTYPE_p_iDynTree__AngularMotionVector3Semantics swig_types[28]
-#define SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers swig_types[29]
-#define SWIGTYPE_p_iDynTree__ArticulatedBodyInertia swig_types[30]
-#define SWIGTYPE_p_iDynTree__Axis swig_types[31]
-#define SWIGTYPE_p_iDynTree__BerdyDynamicVariable swig_types[32]
-#define SWIGTYPE_p_iDynTree__BerdyHelper swig_types[33]
-#define SWIGTYPE_p_iDynTree__BerdyOptions swig_types[34]
-#define SWIGTYPE_p_iDynTree__BerdySensor swig_types[35]
-#define SWIGTYPE_p_iDynTree__BerdySparseMAPSolver swig_types[36]
-#define SWIGTYPE_p_iDynTree__Box swig_types[37]
-#define SWIGTYPE_p_iDynTree__ClassicalAcc swig_types[38]
-#define SWIGTYPE_p_iDynTree__ColorViz swig_types[39]
-#define SWIGTYPE_p_iDynTree__ContactWrench swig_types[40]
-#define SWIGTYPE_p_iDynTree__Cylinder swig_types[41]
-#define SWIGTYPE_p_iDynTree__DOFSpatialForceArray swig_types[42]
-#define SWIGTYPE_p_iDynTree__DOFSpatialMotionArray swig_types[43]
-#define SWIGTYPE_p_iDynTree__Direction swig_types[44]
-#define SWIGTYPE_p_iDynTree__Dummy swig_types[45]
-#define SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator swig_types[46]
-#define SWIGTYPE_p_iDynTree__ExternalMesh swig_types[47]
-#define SWIGTYPE_p_iDynTree__FixedJoint swig_types[48]
-#define SWIGTYPE_p_iDynTree__ForceVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t swig_types[49]
-#define SWIGTYPE_p_iDynTree__ForceVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t swig_types[50]
-#define SWIGTYPE_p_iDynTree__ForceVector3T_iDynTree__AngularForceVector3_t swig_types[51]
-#define SWIGTYPE_p_iDynTree__ForceVector3T_iDynTree__LinearForceVector3_t swig_types[52]
-#define SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian swig_types[53]
-#define SWIGTYPE_p_iDynTree__FreeFloatingAcc swig_types[54]
-#define SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques swig_types[55]
-#define SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix swig_types[56]
-#define SWIGTYPE_p_iDynTree__FreeFloatingPos swig_types[57]
-#define SWIGTYPE_p_iDynTree__FreeFloatingVel swig_types[58]
-#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t swig_types[59]
-#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularMotionVector3Semantics_t swig_types[60]
-#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t swig_types[61]
-#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearMotionVector3Semantics_t swig_types[62]
-#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__AngularForceVector3_t swig_types[63]
-#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__AngularMotionVector3_t swig_types[64]
-#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_t swig_types[65]
-#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t swig_types[66]
-#define SWIGTYPE_p_iDynTree__GyroscopeSensor swig_types[67]
-#define SWIGTYPE_p_iDynTree__HighLevel__DynamicsComputations swig_types[68]
-#define SWIGTYPE_p_iDynTree__ICamera swig_types[69]
-#define SWIGTYPE_p_iDynTree__IEnvironment swig_types[70]
-#define SWIGTYPE_p_iDynTree__IJetsVisualization swig_types[71]
-#define SWIGTYPE_p_iDynTree__IJoint swig_types[72]
-#define SWIGTYPE_p_iDynTree__ILight swig_types[73]
-#define SWIGTYPE_p_iDynTree__IModelVisualization swig_types[74]
-#define SWIGTYPE_p_iDynTree__IndexRange swig_types[75]
-#define SWIGTYPE_p_iDynTree__JointDOFsDoubleArray swig_types[76]
-#define SWIGTYPE_p_iDynTree__JointPosDoubleArray swig_types[77]
-#define SWIGTYPE_p_iDynTree__JointSensor swig_types[78]
-#define SWIGTYPE_p_iDynTree__KinDynComputations swig_types[79]
-#define SWIGTYPE_p_iDynTree__LinearForceVector3 swig_types[80]
-#define SWIGTYPE_p_iDynTree__LinearForceVector3Semantics swig_types[81]
-#define SWIGTYPE_p_iDynTree__LinearMotionVector3 swig_types[82]
-#define SWIGTYPE_p_iDynTree__LinearMotionVector3Semantics swig_types[83]
-#define SWIGTYPE_p_iDynTree__Link swig_types[84]
-#define SWIGTYPE_p_iDynTree__LinkAccArray swig_types[85]
-#define SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias swig_types[86]
-#define SWIGTYPE_p_iDynTree__LinkContactWrenches swig_types[87]
-#define SWIGTYPE_p_iDynTree__LinkInertias swig_types[88]
-#define SWIGTYPE_p_iDynTree__LinkPositions swig_types[89]
-#define SWIGTYPE_p_iDynTree__LinkSensor swig_types[90]
-#define SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts swig_types[91]
-#define SWIGTYPE_p_iDynTree__LinkVelArray swig_types[92]
-#define SWIGTYPE_p_iDynTree__LinkWrenches swig_types[93]
-#define SWIGTYPE_p_iDynTree__MatrixDynSize swig_types[94]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_10_16_t swig_types[95]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_1_6_t swig_types[96]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_2_3_t swig_types[97]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_3_t swig_types[98]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_4_t swig_types[99]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_4_3_t swig_types[100]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_4_4_t swig_types[101]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_10_t swig_types[102]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_1_t swig_types[103]
-#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_6_t swig_types[104]
-#define SWIGTYPE_p_iDynTree__Model swig_types[105]
-#define SWIGTYPE_p_iDynTree__ModelLoader swig_types[106]
-#define SWIGTYPE_p_iDynTree__ModelParserOptions swig_types[107]
-#define SWIGTYPE_p_iDynTree__ModelSolidShapes swig_types[108]
-#define SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian swig_types[109]
-#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t swig_types[110]
-#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t swig_types[111]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t swig_types[112]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t swig_types[113]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t swig_types[114]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t swig_types[115]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t swig_types[116]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t swig_types[117]
-#define SWIGTYPE_p_iDynTree__Neighbor swig_types[118]
-#define SWIGTYPE_p_iDynTree__Position swig_types[119]
-#define SWIGTYPE_p_iDynTree__PositionRaw swig_types[120]
-#define SWIGTYPE_p_iDynTree__PositionSemantics swig_types[121]
-#define SWIGTYPE_p_iDynTree__PrismaticJoint swig_types[122]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorGenerator swig_types[123]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParameter swig_types[124]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParametersList swig_types[125]
-#define SWIGTYPE_p_iDynTree__RevoluteJoint swig_types[126]
-#define SWIGTYPE_p_iDynTree__RigidBodyInertiaNonLinearParametrization swig_types[127]
-#define SWIGTYPE_p_iDynTree__Rotation swig_types[128]
-#define SWIGTYPE_p_iDynTree__RotationRaw swig_types[129]
-#define SWIGTYPE_p_iDynTree__RotationSemantics swig_types[130]
-#define SWIGTYPE_p_iDynTree__RotationalInertiaRaw swig_types[131]
-#define SWIGTYPE_p_iDynTree__Sensor swig_types[132]
-#define SWIGTYPE_p_iDynTree__SensorsList swig_types[133]
-#define SWIGTYPE_p_iDynTree__SensorsMeasurements swig_types[134]
-#define SWIGTYPE_p_iDynTree__SimpleLeggedOdometry swig_types[135]
-#define SWIGTYPE_p_iDynTree__SixAxisForceTorqueSensor swig_types[136]
-#define SWIGTYPE_p_iDynTree__SolidShape swig_types[137]
-#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t swig_types[138]
-#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t swig_types[139]
-#define SWIGTYPE_p_iDynTree__SpatialAcc swig_types[140]
-#define SWIGTYPE_p_iDynTree__SpatialForceVector swig_types[141]
-#define SWIGTYPE_p_iDynTree__SpatialInertia swig_types[142]
-#define SWIGTYPE_p_iDynTree__SpatialInertiaRaw swig_types[143]
-#define SWIGTYPE_p_iDynTree__SpatialMomentum swig_types[144]
-#define SWIGTYPE_p_iDynTree__SpatialMotionVector swig_types[145]
-#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearForceVector3Semantics_iDynTree__AngularForceVector3Semantics_t swig_types[146]
-#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearMotionVector3Semantics_iDynTree__AngularMotionVector3Semantics_t swig_types[147]
-#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t swig_types[148]
-#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t swig_types[149]
-#define SWIGTYPE_p_iDynTree__Sphere swig_types[150]
-#define SWIGTYPE_p_iDynTree__SubModelDecomposition swig_types[151]
-#define SWIGTYPE_p_iDynTree__ThreeAxisAngularAccelerometerSensor swig_types[152]
-#define SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor swig_types[153]
-#define SWIGTYPE_p_iDynTree__Transform swig_types[154]
-#define SWIGTYPE_p_iDynTree__TransformDerivative swig_types[155]
-#define SWIGTYPE_p_iDynTree__TransformSemantics swig_types[156]
-#define SWIGTYPE_p_iDynTree__Traversal swig_types[157]
-#define SWIGTYPE_p_iDynTree__Triplets swig_types[158]
-#define SWIGTYPE_p_iDynTree__Twist swig_types[159]
-#define SWIGTYPE_p_iDynTree__URDFParserOptions swig_types[160]
-#define SWIGTYPE_p_iDynTree__UnknownWrenchContact swig_types[161]
-#define SWIGTYPE_p_iDynTree__VectorDynSize swig_types[162]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_10_t swig_types[163]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_16_t swig_types[164]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_2_t swig_types[165]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t swig_types[166]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t swig_types[167]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t swig_types[168]
-#define SWIGTYPE_p_iDynTree__Visualizer swig_types[169]
-#define SWIGTYPE_p_iDynTree__VisualizerOptions swig_types[170]
-#define SWIGTYPE_p_iDynTree__Wrench swig_types[171]
-#define SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers swig_types[172]
-#define SWIGTYPE_p_int swig_types[173]
-#define SWIGTYPE_p_iterator swig_types[174]
-#define SWIGTYPE_p_pointer swig_types[175]
-#define SWIGTYPE_p_size_type swig_types[176]
-#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t swig_types[177]
-#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdySensor_t swig_types[178]
-#define SWIGTYPE_p_std__allocatorT_std__string_t swig_types[179]
-#define SWIGTYPE_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t swig_types[180]
-#define SWIGTYPE_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t swig_types[181]
-#define SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t swig_types[182]
-#define SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t swig_types[183]
-#define SWIGTYPE_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t swig_types[184]
-#define SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t swig_types[185]
-#define SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t swig_types[186]
-#define SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t swig_types[187]
-#define SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t swig_types[188]
-#define SWIGTYPE_p_swig__MatlabSwigIterator swig_types[189]
-#define SWIGTYPE_p_typed_iterator swig_types[190]
-#define SWIGTYPE_p_unsigned_int swig_types[191]
-#define SWIGTYPE_p_unsigned_long swig_types[192]
-#define SWIGTYPE_p_value_type swig_types[193]
-static swig_type_info *swig_types[195];
-static swig_module_info swig_module = {swig_types, 194, 0, 0, 0, 0};
+#define SWIGTYPE_p_const_reverse_iterator swig_types[21]
+#define SWIGTYPE_p_const_typed_iterator swig_types[22]
+#define SWIGTYPE_p_difference_type swig_types[23]
+#define SWIGTYPE_p_double swig_types[24]
+#define SWIGTYPE_p_element_type swig_types[25]
+#define SWIGTYPE_p_iDynTree__AccelerometerSensor swig_types[26]
+#define SWIGTYPE_p_iDynTree__AngularForceVector3 swig_types[27]
+#define SWIGTYPE_p_iDynTree__AngularForceVector3Semantics swig_types[28]
+#define SWIGTYPE_p_iDynTree__AngularMotionVector3 swig_types[29]
+#define SWIGTYPE_p_iDynTree__AngularMotionVector3Semantics swig_types[30]
+#define SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers swig_types[31]
+#define SWIGTYPE_p_iDynTree__ArticulatedBodyInertia swig_types[32]
+#define SWIGTYPE_p_iDynTree__AttitudeEstimatorState swig_types[33]
+#define SWIGTYPE_p_iDynTree__AttitudeMahonyFilter swig_types[34]
+#define SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters swig_types[35]
+#define SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF swig_types[36]
+#define SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters swig_types[37]
+#define SWIGTYPE_p_iDynTree__Axis swig_types[38]
+#define SWIGTYPE_p_iDynTree__BerdyDynamicVariable swig_types[39]
+#define SWIGTYPE_p_iDynTree__BerdyHelper swig_types[40]
+#define SWIGTYPE_p_iDynTree__BerdyOptions swig_types[41]
+#define SWIGTYPE_p_iDynTree__BerdySensor swig_types[42]
+#define SWIGTYPE_p_iDynTree__BerdySparseMAPSolver swig_types[43]
+#define SWIGTYPE_p_iDynTree__Box swig_types[44]
+#define SWIGTYPE_p_iDynTree__ClassicalAcc swig_types[45]
+#define SWIGTYPE_p_iDynTree__ColorViz swig_types[46]
+#define SWIGTYPE_p_iDynTree__ContactWrench swig_types[47]
+#define SWIGTYPE_p_iDynTree__Cylinder swig_types[48]
+#define SWIGTYPE_p_iDynTree__DOFSpatialForceArray swig_types[49]
+#define SWIGTYPE_p_iDynTree__DOFSpatialMotionArray swig_types[50]
+#define SWIGTYPE_p_iDynTree__Direction swig_types[51]
+#define SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper swig_types[52]
+#define SWIGTYPE_p_iDynTree__Dummy swig_types[53]
+#define SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator swig_types[54]
+#define SWIGTYPE_p_iDynTree__ExternalMesh swig_types[55]
+#define SWIGTYPE_p_iDynTree__FixedJoint swig_types[56]
+#define SWIGTYPE_p_iDynTree__ForceVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t swig_types[57]
+#define SWIGTYPE_p_iDynTree__ForceVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t swig_types[58]
+#define SWIGTYPE_p_iDynTree__ForceVector3T_iDynTree__AngularForceVector3_t swig_types[59]
+#define SWIGTYPE_p_iDynTree__ForceVector3T_iDynTree__LinearForceVector3_t swig_types[60]
+#define SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian swig_types[61]
+#define SWIGTYPE_p_iDynTree__FreeFloatingAcc swig_types[62]
+#define SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques swig_types[63]
+#define SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix swig_types[64]
+#define SWIGTYPE_p_iDynTree__FreeFloatingPos swig_types[65]
+#define SWIGTYPE_p_iDynTree__FreeFloatingVel swig_types[66]
+#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t swig_types[67]
+#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularMotionVector3Semantics_t swig_types[68]
+#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t swig_types[69]
+#define SWIGTYPE_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearMotionVector3Semantics_t swig_types[70]
+#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__AngularForceVector3_t swig_types[71]
+#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__AngularMotionVector3_t swig_types[72]
+#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_t swig_types[73]
+#define SWIGTYPE_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t swig_types[74]
+#define SWIGTYPE_p_iDynTree__GyroscopeSensor swig_types[75]
+#define SWIGTYPE_p_iDynTree__HighLevel__DynamicsComputations swig_types[76]
+#define SWIGTYPE_p_iDynTree__IAttitudeEstimator swig_types[77]
+#define SWIGTYPE_p_iDynTree__ICamera swig_types[78]
+#define SWIGTYPE_p_iDynTree__IEnvironment swig_types[79]
+#define SWIGTYPE_p_iDynTree__IJetsVisualization swig_types[80]
+#define SWIGTYPE_p_iDynTree__IJoint swig_types[81]
+#define SWIGTYPE_p_iDynTree__ILight swig_types[82]
+#define SWIGTYPE_p_iDynTree__IModelVisualization swig_types[83]
+#define SWIGTYPE_p_iDynTree__IVectorsVisualization swig_types[84]
+#define SWIGTYPE_p_iDynTree__IndexRange swig_types[85]
+#define SWIGTYPE_p_iDynTree__JointDOFsDoubleArray swig_types[86]
+#define SWIGTYPE_p_iDynTree__JointPosDoubleArray swig_types[87]
+#define SWIGTYPE_p_iDynTree__JointSensor swig_types[88]
+#define SWIGTYPE_p_iDynTree__KinDynComputations swig_types[89]
+#define SWIGTYPE_p_iDynTree__LinearForceVector3 swig_types[90]
+#define SWIGTYPE_p_iDynTree__LinearForceVector3Semantics swig_types[91]
+#define SWIGTYPE_p_iDynTree__LinearMotionVector3 swig_types[92]
+#define SWIGTYPE_p_iDynTree__LinearMotionVector3Semantics swig_types[93]
+#define SWIGTYPE_p_iDynTree__Link swig_types[94]
+#define SWIGTYPE_p_iDynTree__LinkAccArray swig_types[95]
+#define SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias swig_types[96]
+#define SWIGTYPE_p_iDynTree__LinkContactWrenches swig_types[97]
+#define SWIGTYPE_p_iDynTree__LinkInertias swig_types[98]
+#define SWIGTYPE_p_iDynTree__LinkPositions swig_types[99]
+#define SWIGTYPE_p_iDynTree__LinkSensor swig_types[100]
+#define SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts swig_types[101]
+#define SWIGTYPE_p_iDynTree__LinkVelArray swig_types[102]
+#define SWIGTYPE_p_iDynTree__LinkWrenches swig_types[103]
+#define SWIGTYPE_p_iDynTree__MatrixDynSize swig_types[104]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_10_16_t swig_types[105]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_1_6_t swig_types[106]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_2_3_t swig_types[107]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_3_t swig_types[108]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_4_t swig_types[109]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_4_3_t swig_types[110]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_4_4_t swig_types[111]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_10_t swig_types[112]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_1_t swig_types[113]
+#define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_6_t swig_types[114]
+#define SWIGTYPE_p_iDynTree__Model swig_types[115]
+#define SWIGTYPE_p_iDynTree__ModelLoader swig_types[116]
+#define SWIGTYPE_p_iDynTree__ModelParserOptions swig_types[117]
+#define SWIGTYPE_p_iDynTree__ModelSolidShapes swig_types[118]
+#define SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian swig_types[119]
+#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t swig_types[120]
+#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t swig_types[121]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t swig_types[122]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t swig_types[123]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t swig_types[124]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t swig_types[125]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t swig_types[126]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t swig_types[127]
+#define SWIGTYPE_p_iDynTree__Neighbor swig_types[128]
+#define SWIGTYPE_p_iDynTree__Position swig_types[129]
+#define SWIGTYPE_p_iDynTree__PositionRaw swig_types[130]
+#define SWIGTYPE_p_iDynTree__PositionSemantics swig_types[131]
+#define SWIGTYPE_p_iDynTree__PrismaticJoint swig_types[132]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorGenerator swig_types[133]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParameter swig_types[134]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParametersList swig_types[135]
+#define SWIGTYPE_p_iDynTree__RevoluteJoint swig_types[136]
+#define SWIGTYPE_p_iDynTree__RigidBodyInertiaNonLinearParametrization swig_types[137]
+#define SWIGTYPE_p_iDynTree__Rotation swig_types[138]
+#define SWIGTYPE_p_iDynTree__RotationRaw swig_types[139]
+#define SWIGTYPE_p_iDynTree__RotationSemantics swig_types[140]
+#define SWIGTYPE_p_iDynTree__RotationalInertiaRaw swig_types[141]
+#define SWIGTYPE_p_iDynTree__Sensor swig_types[142]
+#define SWIGTYPE_p_iDynTree__SensorsList swig_types[143]
+#define SWIGTYPE_p_iDynTree__SensorsMeasurements swig_types[144]
+#define SWIGTYPE_p_iDynTree__SimpleLeggedOdometry swig_types[145]
+#define SWIGTYPE_p_iDynTree__SixAxisForceTorqueSensor swig_types[146]
+#define SWIGTYPE_p_iDynTree__SolidShape swig_types[147]
+#define SWIGTYPE_p_iDynTree__SpanT_double__1_t swig_types[148]
+#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t swig_types[149]
+#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t swig_types[150]
+#define SWIGTYPE_p_iDynTree__SpatialAcc swig_types[151]
+#define SWIGTYPE_p_iDynTree__SpatialForceVector swig_types[152]
+#define SWIGTYPE_p_iDynTree__SpatialInertia swig_types[153]
+#define SWIGTYPE_p_iDynTree__SpatialInertiaRaw swig_types[154]
+#define SWIGTYPE_p_iDynTree__SpatialMomentum swig_types[155]
+#define SWIGTYPE_p_iDynTree__SpatialMotionVector swig_types[156]
+#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearForceVector3Semantics_iDynTree__AngularForceVector3Semantics_t swig_types[157]
+#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearMotionVector3Semantics_iDynTree__AngularMotionVector3Semantics_t swig_types[158]
+#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t swig_types[159]
+#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t swig_types[160]
+#define SWIGTYPE_p_iDynTree__Sphere swig_types[161]
+#define SWIGTYPE_p_iDynTree__SubModelDecomposition swig_types[162]
+#define SWIGTYPE_p_iDynTree__ThreeAxisAngularAccelerometerSensor swig_types[163]
+#define SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor swig_types[164]
+#define SWIGTYPE_p_iDynTree__Transform swig_types[165]
+#define SWIGTYPE_p_iDynTree__TransformDerivative swig_types[166]
+#define SWIGTYPE_p_iDynTree__TransformSemantics swig_types[167]
+#define SWIGTYPE_p_iDynTree__Traversal swig_types[168]
+#define SWIGTYPE_p_iDynTree__Triplets swig_types[169]
+#define SWIGTYPE_p_iDynTree__Twist swig_types[170]
+#define SWIGTYPE_p_iDynTree__URDFParserOptions swig_types[171]
+#define SWIGTYPE_p_iDynTree__UnknownWrenchContact swig_types[172]
+#define SWIGTYPE_p_iDynTree__VectorDynSize swig_types[173]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_10_t swig_types[174]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_16_t swig_types[175]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_2_t swig_types[176]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t swig_types[177]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t swig_types[178]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t swig_types[179]
+#define SWIGTYPE_p_iDynTree__Visualizer swig_types[180]
+#define SWIGTYPE_p_iDynTree__VisualizerOptions swig_types[181]
+#define SWIGTYPE_p_iDynTree__Wrench swig_types[182]
+#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t swig_types[183]
+#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t swig_types[184]
+#define SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers swig_types[185]
+#define SWIGTYPE_p_index_type swig_types[186]
+#define SWIGTYPE_p_int swig_types[187]
+#define SWIGTYPE_p_iterator swig_types[188]
+#define SWIGTYPE_p_pointer swig_types[189]
+#define SWIGTYPE_p_reference swig_types[190]
+#define SWIGTYPE_p_reverse_iterator swig_types[191]
+#define SWIGTYPE_p_size_type swig_types[192]
+#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t swig_types[193]
+#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdySensor_t swig_types[194]
+#define SWIGTYPE_p_std__allocatorT_std__string_t swig_types[195]
+#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t swig_types[196]
+#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t swig_types[197]
+#define SWIGTYPE_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t swig_types[198]
+#define SWIGTYPE_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t swig_types[199]
+#define SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t swig_types[200]
+#define SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t swig_types[201]
+#define SWIGTYPE_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t swig_types[202]
+#define SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t swig_types[203]
+#define SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t swig_types[204]
+#define SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t swig_types[205]
+#define SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t swig_types[206]
+#define SWIGTYPE_p_swig__MatlabSwigIterator swig_types[207]
+#define SWIGTYPE_p_typed_iterator swig_types[208]
+#define SWIGTYPE_p_unsigned_int swig_types[209]
+#define SWIGTYPE_p_unsigned_long swig_types[210]
+#define SWIGTYPE_p_value_type swig_types[211]
+static swig_type_info *swig_types[213];
+static swig_module_info swig_module = {swig_types, 212, 0, 0, 0, 0};
 #define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name)
 #define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name)
 
@@ -2923,6 +2941,7 @@ SWIGINTERN void std_vector_Sl_std_string_Sg__insert__SWIG_1(std::vector< std::st
 #include "iDynTree/Core/TransformSemantics.h"
 #include "iDynTree/Core/Transform.h"
 #include "iDynTree/Core/TransformDerivative.h"
+#include "iDynTree/Core/Span.h"
 
 // Model related data structures
 #include "iDynTree/Model/Indices.h"
@@ -2966,6 +2985,10 @@ SWIGINTERN void std_vector_Sl_std_string_Sg__insert__SWIG_1(std::vector< std::st
 #include "iDynTree/Estimation/SimpleLeggedOdometry.h"
 #include "iDynTree/Estimation/BerdyHelper.h"
 #include "iDynTree/Estimation/BerdySparseMAPSolver.h"
+#include "iDynTree/Estimation/AttitudeEstimator.h"
+#include "iDynTree/Estimation/AttitudeMahonyFilter.h"
+#include "iDynTree/Estimation/ExtendedKalmanFilter.h"
+#include "iDynTree/Estimation/AttitudeQuaternionEKF.h"
 
 // Regressors related data structures
 #include "iDynTree/Regressors/DynamicsRegressorParameters.h"
@@ -3136,7 +3159,7 @@ SWIGINTERN void iDynTree_MatrixDynSize_fromMatlab(iDynTree::MatrixDynSize *self,
 
         if (mxIsSparse(in))
         {
-            /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+            /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3402,7 +3425,7 @@ SWIGINTERN void iDynTree_VectorDynSize_fromMatlab(iDynTree::VectorDynSize *self,
 
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3464,7 +3487,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_1_Sc_6_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3527,7 +3550,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_2_Sc_3_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3590,7 +3613,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_3_Sc_3_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3653,7 +3676,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_4_Sc_4_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3716,7 +3739,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_6_Sc_6_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3779,7 +3802,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_6_Sc_10_Sg__fromMatlab(iDynTree::Matri
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3842,7 +3865,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_10_Sc_16_Sg__fromMatlab(iDynTree::Matr
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3906,7 +3929,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_3_Sg__fromMatlab(iDynTree::VectorFixSi
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3972,7 +3995,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_4_Sg__fromMatlab(iDynTree::VectorFixSi
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -4038,7 +4061,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_6_Sg__fromMatlab(iDynTree::VectorFixSi
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -4104,7 +4127,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_10_Sg__fromMatlab(iDynTree::VectorFixS
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -4170,7 +4193,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_16_Sg__fromMatlab(iDynTree::VectorFixS
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -40582,233 +40605,22 @@ int _wrap_TransformDerivative_transform(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-SWIGINTERN int _wrap_LINK_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::LINK_INVALID_INDEX));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_LINK_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    int val;
-    int res = SWIG_AsVal_int(argv[0], &val);
-    if (!SWIG_IsOK(res)) {
-      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::LINK_INVALID_INDEX""' of type '""iDynTree::LinkIndex""'");
-    }
-    iDynTree::LINK_INVALID_INDEX = static_cast< iDynTree::LinkIndex >(val);
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_LINK_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::LINK_INVALID_NAME));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_LINK_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::LINK_INVALID_NAME""' of type '""std::string""'"); 
-    }
-    iDynTree::LINK_INVALID_NAME = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_JOINT_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::JOINT_INVALID_INDEX));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_JOINT_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    int val;
-    int res = SWIG_AsVal_int(argv[0], &val);
-    if (!SWIG_IsOK(res)) {
-      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::JOINT_INVALID_INDEX""' of type '""int""'");
-    }
-    iDynTree::JOINT_INVALID_INDEX = static_cast< int >(val);
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_JOINT_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::JOINT_INVALID_NAME));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_JOINT_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::JOINT_INVALID_NAME""' of type '""std::string""'"); 
-    }
-    iDynTree::JOINT_INVALID_NAME = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_DOF_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::DOF_INVALID_INDEX));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_DOF_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    int val;
-    int res = SWIG_AsVal_int(argv[0], &val);
-    if (!SWIG_IsOK(res)) {
-      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::DOF_INVALID_INDEX""' of type '""int""'");
-    }
-    iDynTree::DOF_INVALID_INDEX = static_cast< int >(val);
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_DOF_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::DOF_INVALID_NAME));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_DOF_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::DOF_INVALID_NAME""' of type '""std::string""'"); 
-    }
-    iDynTree::DOF_INVALID_NAME = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_FRAME_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::FRAME_INVALID_INDEX));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_FRAME_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    int val;
-    int res = SWIG_AsVal_int(argv[0], &val);
-    if (!SWIG_IsOK(res)) {
-      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::FRAME_INVALID_INDEX""' of type '""int""'");
-    }
-    iDynTree::FRAME_INVALID_INDEX = static_cast< int >(val);
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_FRAME_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::FRAME_INVALID_NAME));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_FRAME_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::FRAME_INVALID_NAME""' of type '""std::string""'"); 
-    }
-    iDynTree::FRAME_INVALID_NAME = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-SWIGINTERN int _wrap_TRAVERSAL_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::TRAVERSAL_INVALID_INDEX));
-  return 0;
-}
-
-
-SWIGINTERN int _wrap_TRAVERSAL_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  {
-    int val;
-    int res = SWIG_AsVal_int(argv[0], &val);
-    if (!SWIG_IsOK(res)) {
-      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::TRAVERSAL_INVALID_INDEX""' of type '""iDynTree::TraversalIndex""'");
-    }
-    iDynTree::TRAVERSAL_INVALID_INDEX = static_cast< iDynTree::TraversalIndex >(val);
-  }
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_LinkPositions__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
-  mxArray * _out;
-  iDynTree::LinkPositions *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_LinkPositions",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkPositions" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkPositions *)new iDynTree::LinkPositions(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
+SWIGINTERN int _wrap_dynamic_extent_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_ptrdiff_t(static_cast< ptrdiff_t >(iDynTree::dynamic_extent));
   return 0;
-fail:
-  return 1;
 }
 
 
-int _wrap_new_LinkPositions__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DynamicSpan_extent_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkPositions *result = 0 ;
+  iDynTree::Span< double,-1 >::index_type result;
   
-  if (!SWIG_check_num_args("new_LinkPositions",argc,0,0,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_extent_get",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::LinkPositions *)new iDynTree::LinkPositions();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 1 |  0 );
+  result = (iDynTree::Span< double,-1 >::index_type)iDynTree::Span< double,-1 >::extent;
+  _out = SWIG_From_ptrdiff_t(static_cast< ptrdiff_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -40816,26 +40628,31 @@ int _wrap_new_LinkPositions__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkPositions__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_new_DynamicSpan__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 >::pointer arg1 = (iDynTree::Span< double,-1 >::pointer) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkPositions *result = 0 ;
+  iDynTree::Span< double,-1 > *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkPositions",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_DynamicSpan",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_double, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkPositions" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkPositions" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DynamicSpan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 >::pointer""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkPositions *)new iDynTree::LinkPositions((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 >::pointer >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_DynamicSpan" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = (iDynTree::Span< double,-1 > *)new iDynTree::Span< double,-1 >(arg1,arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpanT_double__1_t, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -40843,63 +40660,31 @@ int _wrap_new_LinkPositions__SWIG_2(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkPositions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_LinkPositions__SWIG_1(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_LinkPositions__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_LinkPositions__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkPositions'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkPositions::LinkPositions(unsigned int)\n"
-    "    iDynTree::LinkPositions::LinkPositions()\n"
-    "    iDynTree::LinkPositions::LinkPositions(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_LinkPositions_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
-  unsigned int arg2 ;
+int _wrap_new_DynamicSpan__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 >::pointer arg1 = (iDynTree::Span< double,-1 >::pointer) 0 ;
+  iDynTree::Span< double,-1 >::pointer arg2 = (iDynTree::Span< double,-1 >::pointer) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
+  iDynTree::Span< double,-1 > *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkPositions_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_DynamicSpan",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_double, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_resize" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DynamicSpan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 >::pointer""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkPositions_resize" "', argument " "2"" of type '" "unsigned int""'");
-  } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 >::pointer >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_double, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_DynamicSpan" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::pointer""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 >::pointer >(argp2);
+  result = (iDynTree::Span< double,-1 > *)new iDynTree::Span< double,-1 >(arg1,arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpanT_double__1_t, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -40907,33 +40692,26 @@ int _wrap_LinkPositions_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_LinkPositions_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_DynamicSpan__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
+  iDynTree::Span< double,-1 > *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkPositions_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_DynamicSpan",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_resize" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DynamicSpan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkPositions_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkPositions_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_DynamicSpan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = (iDynTree::Span< double,-1 > *)new iDynTree::Span< double,-1 >((iDynTree::Span< double,-1 > const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpanT_double__1_t, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -40941,73 +40719,107 @@ int _wrap_LinkPositions_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_LinkPositions_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DynamicSpan(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_DynamicSpan__SWIG_2(resc,resv,argc,argv);
+    }
+  }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_double, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_double, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_LinkPositions_resize__SWIG_1(resc,resv,argc,argv);
+        return _wrap_new_DynamicSpan__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_double, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        int res = SWIG_AsVal_ptrdiff_t(argv[1], NULL);
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkPositions_resize__SWIG_0(resc,resv,argc,argv);
+        return _wrap_new_DynamicSpan__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkPositions_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_DynamicSpan'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkPositions::resize(unsigned int)\n"
-    "    iDynTree::LinkPositions::resize(iDynTree::Model const &)\n");
+    "    iDynTree::Span< double,-1 >::Span(iDynTree::Span< double,-1 >::pointer,iDynTree::Span< double,-1 >::index_type)\n"
+    "    iDynTree::Span< double,-1 >::Span(iDynTree::Span< double,-1 >::pointer,iDynTree::Span< double,-1 >::pointer)\n"
+    "    iDynTree::Span< double,-1 >::Span(iDynTree::Span< double,-1 > const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkPositions_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_delete_DynamicSpan(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("LinkPositions_isConsistent",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_DynamicSpan",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_DynamicSpan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkPositions_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkPositions_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DynamicSpan_first(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  SwigValueWrapper< iDynTree::Span< double,-1 > > result;
+  
+  if (!SWIG_check_num_args("DynamicSpan_first",argc,2,2,0)) {
+    SWIG_fail;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::LinkPositions const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_first" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_first" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->first(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >(static_cast< const iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >& >(result))), SWIGTYPE_p_iDynTree__SpanT_double__1_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41015,23 +40827,31 @@ int _wrap_LinkPositions_isConsistent(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkPositions_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
+int _wrap_DynamicSpan_last(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  size_t result;
+  SwigValueWrapper< iDynTree::Span< double,-1 > > result;
   
-  if (!SWIG_check_num_args("LinkPositions_getNrOfLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_last",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_last" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  result = ((iDynTree::LinkPositions const *)arg1)->getNrOfLinks();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_last" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->last(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >(static_cast< const iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >& >(result))), SWIGTYPE_p_iDynTree__SpanT_double__1_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41039,31 +40859,39 @@ int _wrap_LinkPositions_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkPositions_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_DynamicSpan_subspan__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
+  iDynTree::Span< double,-1 >::index_type arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  ptrdiff_t val2 ;
   int ecode2 = 0 ;
+  ptrdiff_t val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
+  SwigValueWrapper< iDynTree::Span< double,-1 > > result;
   
-  if (!SWIG_check_num_args("LinkPositions_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_subspan",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_paren" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_subspan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkPositions_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_subspan" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
   } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Transform *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  ecode3 = SWIG_AsVal_ptrdiff_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "DynamicSpan_subspan" "', argument " "3"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg3 = static_cast< iDynTree::Span< double,-1 >::index_type >(val3);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->subspan(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >(static_cast< const iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >& >(result))), SWIGTYPE_p_iDynTree__SpanT_double__1_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41071,31 +40899,31 @@ int _wrap_LinkPositions_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkPositions_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_DynamicSpan_subspan__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  ptrdiff_t val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
+  SwigValueWrapper< iDynTree::Span< double,-1 > > result;
   
-  if (!SWIG_check_num_args("LinkPositions_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_subspan",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_paren" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_subspan" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkPositions_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_subspan" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
   } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Transform *) &((iDynTree::LinkPositions const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->subspan(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >(static_cast< const iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 >& >(result))), SWIGTYPE_p_iDynTree__SpanT_double__1_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41103,74 +40931,69 @@ int _wrap_LinkPositions_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkPositions_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DynamicSpan_subspan(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
-        int res = SWIG_AsVal_int(argv[1], NULL);
+        int res = SWIG_AsVal_ptrdiff_t(argv[1], NULL);
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkPositions_paren__SWIG_0(resc,resv,argc,argv);
+        return _wrap_DynamicSpan_subspan__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 2) {
+  if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
-        int res = SWIG_AsVal_int(argv[1], NULL);
+        int res = SWIG_AsVal_ptrdiff_t(argv[1], NULL);
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkPositions_paren__SWIG_1(resc,resv,argc,argv);
+        {
+          int res = SWIG_AsVal_ptrdiff_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_DynamicSpan_subspan__SWIG_0(resc,resv,argc,argv);
+        }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkPositions_paren'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DynamicSpan_subspan'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkPositions::operator ()(iDynTree::LinkIndex const)\n"
-    "    iDynTree::LinkPositions::operator ()(iDynTree::LinkIndex const) const\n");
+    "    iDynTree::Span< double,-1 >::subspan(iDynTree::Span< double,-1 >::index_type,iDynTree::Span< double,-1 >::index_type) const\n"
+    "    iDynTree::Span< double,-1 >::subspan(iDynTree::Span< double,-1 >::index_type) const\n");
   return 1;
 }
 
 
-int _wrap_LinkPositions_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_DynamicSpan_size(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  std::string result;
+  iDynTree::Span< double,-1 >::index_type result;
   
-  if (!SWIG_check_num_args("LinkPositions_toString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_size",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_toString" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkPositions_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkPositions_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_size" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::LinkPositions const *)arg1)->toString((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->size();
+  _out = SWIG_From_ptrdiff_t(static_cast< ptrdiff_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41178,26 +41001,23 @@ int _wrap_LinkPositions_toString(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_delete_LinkPositions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
+int _wrap_DynamicSpan_size_bytes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::Span< double,-1 >::index_type result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_LinkPositions",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_size_bytes",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkPositions" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_size_bytes" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->size_bytes();
+  _out = SWIG_From_ptrdiff_t(static_cast< ptrdiff_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41205,23 +41025,23 @@ int _wrap_delete_LinkPositions(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_new_LinkWrenches__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
+int _wrap_DynamicSpan_empty(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkWrenches *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_LinkWrenches",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_empty",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkWrenches" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkWrenches *)new iDynTree::LinkWrenches(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_empty" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = (bool)((iDynTree::Span< double,-1 > const *)arg1)->empty();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41229,43 +41049,63 @@ int _wrap_new_LinkWrenches__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkWrenches__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DynamicSpan_brace(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkWrenches *result = 0 ;
+  iDynTree::Span< double,-1 >::element_type *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkWrenches",argc,0,0,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_brace",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::LinkWrenches *)new iDynTree::LinkWrenches();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_brace" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_brace" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = (iDynTree::Span< double,-1 >::element_type *) &((iDynTree::Span< double,-1 > const *)arg1)->operator [](arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_double, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_new_LinkWrenches__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_DynamicSpan_getVal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkWrenches *result = 0 ;
+  double result;
   
-  if (!SWIG_check_num_args("new_LinkWrenches",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_getVal",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_getVal" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkWrenches *)new iDynTree::LinkWrenches((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_getVal" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = (double)((iDynTree::Span< double,-1 > const *)arg1)->getVal(arg2);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41273,63 +41113,39 @@ int _wrap_new_LinkWrenches__SWIG_2(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_LinkWrenches__SWIG_1(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_LinkWrenches__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_LinkWrenches__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkWrenches'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkWrenches::LinkWrenches(unsigned int)\n"
-    "    iDynTree::LinkWrenches::LinkWrenches()\n"
-    "    iDynTree::LinkWrenches::LinkWrenches(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_LinkWrenches_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
-  unsigned int arg2 ;
+int _wrap_DynamicSpan_setVal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
+  double arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
+  ptrdiff_t val2 ;
   int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("LinkWrenches_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_setVal",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_setVal" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkWrenches_resize" "', argument " "2"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_setVal" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
   } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "DynamicSpan_setVal" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  result = (bool)(arg1)->setVal(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41337,33 +41153,31 @@ int _wrap_LinkWrenches_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkWrenches_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_DynamicSpan_at(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::Span< double,-1 >::element_type *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkWrenches_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_at",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_at" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_at" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = (iDynTree::Span< double,-1 >::element_type *) &((iDynTree::Span< double,-1 > const *)arg1)->at(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_double, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41371,73 +41185,31 @@ int _wrap_LinkWrenches_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkWrenches_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_LinkWrenches_resize__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkWrenches_resize__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkWrenches_resize'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkWrenches::resize(unsigned int)\n"
-    "    iDynTree::LinkWrenches::resize(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_LinkWrenches_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_DynamicSpan_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  iDynTree::Span< double,-1 >::index_type arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  ptrdiff_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::Span< double,-1 >::element_type *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkWrenches_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkWrenches_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkWrenches_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_paren" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::LinkWrenches const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  ecode2 = SWIG_AsVal_ptrdiff_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DynamicSpan_paren" "', argument " "2"" of type '" "iDynTree::Span< double,-1 >::index_type""'");
+  } 
+  arg2 = static_cast< iDynTree::Span< double,-1 >::index_type >(val2);
+  result = (iDynTree::Span< double,-1 >::element_type *) &((iDynTree::Span< double,-1 > const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_double, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41445,23 +41217,23 @@ int _wrap_LinkWrenches_isConsistent(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_LinkWrenches_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
+int _wrap_DynamicSpan_begin(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  SwigValueWrapper< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,false > > result;
   
-  if (!SWIG_check_num_args("LinkWrenches_getNrOfLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_begin",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_begin" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  result = ((iDynTree::LinkWrenches const *)arg1)->getNrOfLinks();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->begin();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::iterator(static_cast< const iDynTree::Span< double,-1 >::iterator& >(result))), SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41469,31 +41241,23 @@ int _wrap_LinkWrenches_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_LinkWrenches_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_DynamicSpan_end(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  SwigValueWrapper< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,false > > result;
   
-  if (!SWIG_check_num_args("LinkWrenches_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_end",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_paren" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_end" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkWrenches_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Wrench *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->end();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::iterator(static_cast< const iDynTree::Span< double,-1 >::iterator& >(result))), SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41501,31 +41265,23 @@ int _wrap_LinkWrenches_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkWrenches_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_DynamicSpan_cbegin(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  SwigValueWrapper< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,true > > result;
   
-  if (!SWIG_check_num_args("LinkWrenches_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_cbegin",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_paren" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_cbegin" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkWrenches_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Wrench *) &((iDynTree::LinkWrenches const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->cbegin();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::const_iterator(static_cast< const iDynTree::Span< double,-1 >::const_iterator& >(result))), SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41533,74 +41289,23 @@ int _wrap_LinkWrenches_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkWrenches_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkWrenches_paren__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkWrenches_paren__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkWrenches_paren'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkWrenches::operator ()(iDynTree::LinkIndex const)\n"
-    "    iDynTree::LinkWrenches::operator ()(iDynTree::LinkIndex const) const\n");
-  return 1;
-}
-
-
-int _wrap_LinkWrenches_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_DynamicSpan_cend(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  std::string result;
+  SwigValueWrapper< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,true > > result;
   
-  if (!SWIG_check_num_args("LinkWrenches_toString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_cend",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_toString" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_cend" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::LinkWrenches const *)arg1)->toString((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->cend();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::const_iterator(static_cast< const iDynTree::Span< double,-1 >::const_iterator& >(result))), SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41608,22 +41313,23 @@ int _wrap_LinkWrenches_toString(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_LinkWrenches_zero(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
+int _wrap_DynamicSpan_rbegin(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::Span< double,-1 >::reverse_iterator result;
   
-  if (!SWIG_check_num_args("LinkWrenches_zero",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_rbegin",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_zero" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_rbegin" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  (arg1)->zero();
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->rbegin();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::reverse_iterator(static_cast< const iDynTree::Span< double,-1 >::reverse_iterator& >(result))), SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41631,26 +41337,23 @@ int _wrap_LinkWrenches_zero(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_delete_LinkWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
+int _wrap_DynamicSpan_rend(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::Span< double,-1 >::reverse_iterator result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_LinkWrenches",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_rend",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkWrenches" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_rend" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->rend();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::reverse_iterator(static_cast< const iDynTree::Span< double,-1 >::reverse_iterator& >(result))), SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41658,23 +41361,23 @@ int _wrap_delete_LinkWrenches(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_new_LinkInertias__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
+int _wrap_DynamicSpan_crbegin(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkInertias *result = 0 ;
+  iDynTree::Span< double,-1 >::const_reverse_iterator result;
   
-  if (!SWIG_check_num_args("new_LinkInertias",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_crbegin",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkInertias" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkInertias *)new iDynTree::LinkInertias(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkInertias, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_crbegin" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->crbegin();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::const_reverse_iterator(static_cast< const iDynTree::Span< double,-1 >::const_reverse_iterator& >(result))), SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41682,16 +41385,23 @@ int _wrap_new_LinkInertias__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkInertias__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DynamicSpan_crend(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Span< double,-1 > *arg1 = (iDynTree::Span< double,-1 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkInertias *result = 0 ;
+  iDynTree::Span< double,-1 >::const_reverse_iterator result;
   
-  if (!SWIG_check_num_args("new_LinkInertias",argc,0,0,0)) {
+  if (!SWIG_check_num_args("DynamicSpan_crend",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::LinkInertias *)new iDynTree::LinkInertias();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkInertias, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DynamicSpan_crend" "', argument " "1"" of type '" "iDynTree::Span< double,-1 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp1);
+  result = ((iDynTree::Span< double,-1 > const *)arg1)->crend();
+  _out = SWIG_NewPointerObj((new iDynTree::Span< double,-1 >::const_reverse_iterator(static_cast< const iDynTree::Span< double,-1 >::const_reverse_iterator& >(result))), SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -41699,353 +41409,216 @@ int _wrap_new_LinkInertias__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkInertias__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::LinkInertias *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_LinkInertias",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+SWIGINTERN int _wrap_LINK_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::LINK_INVALID_INDEX));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_LINK_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    int val;
+    int res = SWIG_AsVal_int(argv[0], &val);
+    if (!SWIG_IsOK(res)) {
+      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::LINK_INVALID_INDEX""' of type '""iDynTree::LinkIndex""'");
+    }
+    iDynTree::LINK_INVALID_INDEX = static_cast< iDynTree::LinkIndex >(val);
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkInertias *)new iDynTree::LinkInertias((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkInertias, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_new_LinkInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_LinkInertias__SWIG_1(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_LinkInertias__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_LinkInertias__SWIG_0(resc,resv,argc,argv);
+SWIGINTERN int _wrap_LINK_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::LINK_INVALID_NAME));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_LINK_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::LINK_INVALID_NAME""' of type '""std::string""'"); 
     }
+    iDynTree::LINK_INVALID_NAME = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkInertias'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkInertias::LinkInertias(unsigned int)\n"
-    "    iDynTree::LinkInertias::LinkInertias()\n"
-    "    iDynTree::LinkInertias::LinkInertias(iDynTree::Model const &)\n");
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_LinkInertias_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
-  unsigned int arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("LinkInertias_resize",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
+SWIGINTERN int _wrap_JOINT_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::JOINT_INVALID_INDEX));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_JOINT_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    int val;
+    int res = SWIG_AsVal_int(argv[0], &val);
+    if (!SWIG_IsOK(res)) {
+      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::JOINT_INVALID_INDEX""' of type '""int""'");
+    }
+    iDynTree::JOINT_INVALID_INDEX = static_cast< int >(val);
   }
-  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkInertias_resize" "', argument " "2"" of type '" "unsigned int""'");
-  } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_LinkInertias_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("LinkInertias_resize",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+SWIGINTERN int _wrap_JOINT_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::JOINT_INVALID_NAME));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_JOINT_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::JOINT_INVALID_NAME""' of type '""std::string""'"); 
+    }
+    iDynTree::JOINT_INVALID_NAME = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_LinkInertias_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_LinkInertias_resize__SWIG_1(resc,resv,argc,argv);
-      }
+SWIGINTERN int _wrap_DOF_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::DOF_INVALID_INDEX));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_DOF_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    int val;
+    int res = SWIG_AsVal_int(argv[0], &val);
+    if (!SWIG_IsOK(res)) {
+      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::DOF_INVALID_INDEX""' of type '""int""'");
     }
+    iDynTree::DOF_INVALID_INDEX = static_cast< int >(val);
   }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkInertias_resize__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkInertias_resize'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkInertias::resize(unsigned int)\n"
-    "    iDynTree::LinkInertias::resize(iDynTree::Model const &)\n");
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_LinkInertias_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("LinkInertias_isConsistent",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkInertias const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::LinkInertias const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
+SWIGINTERN int _wrap_DOF_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::DOF_INVALID_NAME));
   return 0;
-fail:
-  return 1;
 }
 
 
-int _wrap_LinkInertias_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  iDynTree::SpatialInertia *result = 0 ;
-  
-  if (!SWIG_check_num_args("LinkInertias_paren",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
+SWIGINTERN int _wrap_DOF_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::DOF_INVALID_NAME""' of type '""std::string""'"); 
+    }
+    iDynTree::DOF_INVALID_NAME = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::SpatialInertia *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_LinkInertias_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  iDynTree::SpatialInertia *result = 0 ;
-  
-  if (!SWIG_check_num_args("LinkInertias_paren",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkInertias const *""'"); 
+SWIGINTERN int _wrap_FRAME_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::FRAME_INVALID_INDEX));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_FRAME_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    int val;
+    int res = SWIG_AsVal_int(argv[0], &val);
+    if (!SWIG_IsOK(res)) {
+      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::FRAME_INVALID_INDEX""' of type '""int""'");
+    }
+    iDynTree::FRAME_INVALID_INDEX = static_cast< int >(val);
   }
-  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::SpatialInertia *) &((iDynTree::LinkInertias const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_LinkInertias_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkInertias_paren__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkInertias_paren__SWIG_1(resc,resv,argc,argv);
-      }
+SWIGINTERN int _wrap_FRAME_INVALID_NAME_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_std_string(static_cast< std::string >(iDynTree::FRAME_INVALID_NAME));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_FRAME_INVALID_NAME_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in variable '""iDynTree::FRAME_INVALID_NAME""' of type '""std::string""'"); 
     }
+    iDynTree::FRAME_INVALID_NAME = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkInertias_paren'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkInertias::operator ()(iDynTree::LinkIndex const)\n"
-    "    iDynTree::LinkInertias::operator ()(iDynTree::LinkIndex const) const\n");
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_delete_LinkInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  
-  int is_owned;
-  if (!SWIG_check_num_args("delete_LinkInertias",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, SWIG_POINTER_DISOWN |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkInertias" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
-  if (is_owned) {
-    delete arg1;
+SWIGINTERN int _wrap_TRAVERSAL_INVALID_INDEX_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_int(static_cast< int >(iDynTree::TRAVERSAL_INVALID_INDEX));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_TRAVERSAL_INVALID_INDEX_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  {
+    int val;
+    int res = SWIG_AsVal_int(argv[0], &val);
+    if (!SWIG_IsOK(res)) {
+      SWIG_exception_fail(SWIG_ArgError(res), "in variable '""iDynTree::TRAVERSAL_INVALID_INDEX""' of type '""iDynTree::TraversalIndex""'");
+    }
+    iDynTree::TRAVERSAL_INVALID_INDEX = static_cast< iDynTree::TraversalIndex >(val);
   }
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
   return 1;
 }
 
 
-int _wrap_new_LinkArticulatedBodyInertias__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkPositions__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   unsigned int arg1 ;
   unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkArticulatedBodyInertias *result = 0 ;
+  iDynTree::LinkPositions *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkArticulatedBodyInertias",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkPositions",argc,1,1,0)) {
     SWIG_fail;
   }
   ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkPositions" "', argument " "1"" of type '" "unsigned int""'");
   } 
   arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkArticulatedBodyInertias *)new iDynTree::LinkArticulatedBodyInertias(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 1 |  0 );
+  result = (iDynTree::LinkPositions *)new iDynTree::LinkPositions(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42053,16 +41626,16 @@ int _wrap_new_LinkArticulatedBodyInertias__SWIG_0(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_new_LinkArticulatedBodyInertias__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkPositions__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkArticulatedBodyInertias *result = 0 ;
+  iDynTree::LinkPositions *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkArticulatedBodyInertias",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_LinkPositions",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::LinkArticulatedBodyInertias *)new iDynTree::LinkArticulatedBodyInertias();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 1 |  0 );
+  result = (iDynTree::LinkPositions *)new iDynTree::LinkPositions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42070,26 +41643,26 @@ int _wrap_new_LinkArticulatedBodyInertias__SWIG_1(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_new_LinkArticulatedBodyInertias__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkPositions__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkArticulatedBodyInertias *result = 0 ;
+  iDynTree::LinkPositions *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkArticulatedBodyInertias",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkPositions",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkPositions" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkPositions" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkArticulatedBodyInertias *)new iDynTree::LinkArticulatedBodyInertias((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 1 |  0 );
+  result = (iDynTree::LinkPositions *)new iDynTree::LinkPositions((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42097,9 +41670,9 @@ int _wrap_new_LinkArticulatedBodyInertias__SWIG_2(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_new_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkPositions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_LinkArticulatedBodyInertias__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_LinkPositions__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -42107,7 +41680,7 @@ int _wrap_new_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc, m
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_LinkArticulatedBodyInertias__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_LinkPositions__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
@@ -42117,21 +41690,21 @@ int _wrap_new_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc, m
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_LinkArticulatedBodyInertias__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_LinkPositions__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkArticulatedBodyInertias'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkPositions'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkArticulatedBodyInertias::LinkArticulatedBodyInertias(unsigned int)\n"
-    "    iDynTree::LinkArticulatedBodyInertias::LinkArticulatedBodyInertias()\n"
-    "    iDynTree::LinkArticulatedBodyInertias::LinkArticulatedBodyInertias(iDynTree::Model const &)\n");
+    "    iDynTree::LinkPositions::LinkPositions(unsigned int)\n"
+    "    iDynTree::LinkPositions::LinkPositions()\n"
+    "    iDynTree::LinkPositions::LinkPositions(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+int _wrap_LinkPositions_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
   unsigned int arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42139,17 +41712,17 @@ int _wrap_LinkArticulatedBodyInertias_resize__SWIG_0(int resc, mxArray *resv[],
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkPositions_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_resize" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
   ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "2"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkPositions_resize" "', argument " "2"" of type '" "unsigned int""'");
   } 
   arg2 = static_cast< unsigned int >(val2);
   (arg1)->resize(arg2);
@@ -42161,8 +41734,8 @@ int _wrap_LinkArticulatedBodyInertias_resize__SWIG_0(int resc, mxArray *resv[],
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+int _wrap_LinkPositions_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42170,20 +41743,20 @@ int _wrap_LinkArticulatedBodyInertias_resize__SWIG_1(int resc, mxArray *resv[],
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkPositions_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_resize" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkPositions_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkPositions_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -42195,25 +41768,25 @@ int _wrap_LinkArticulatedBodyInertias_resize__SWIG_1(int resc, mxArray *resv[],
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_LinkPositions_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
       int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_LinkArticulatedBodyInertias_resize__SWIG_1(resc,resv,argc,argv);
+        return _wrap_LinkPositions_resize__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -42221,21 +41794,21 @@ int _wrap_LinkArticulatedBodyInertias_resize(int resc, mxArray *resv[], int argc
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkArticulatedBodyInertias_resize__SWIG_0(resc,resv,argc,argv);
+        return _wrap_LinkPositions_resize__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkArticulatedBodyInertias_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkPositions_resize'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkArticulatedBodyInertias::resize(unsigned int)\n"
-    "    iDynTree::LinkArticulatedBodyInertias::resize(iDynTree::Model const &)\n");
+    "    iDynTree::LinkPositions::resize(unsigned int)\n"
+    "    iDynTree::LinkPositions::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+int _wrap_LinkPositions_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42244,23 +41817,23 @@ int _wrap_LinkArticulatedBodyInertias_isConsistent(int resc, mxArray *resv[], in
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkPositions_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkArticulatedBodyInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkPositions_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkArticulatedBodyInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkPositions_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::LinkArticulatedBodyInertias const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  result = (bool)((iDynTree::LinkPositions const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -42269,31 +41842,55 @@ int _wrap_LinkArticulatedBodyInertias_isConsistent(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+int _wrap_LinkPositions_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("LinkPositions_getNrOfLinks",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
+  result = ((iDynTree::LinkPositions const *)arg1)->getNrOfLinks();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkPositions_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::ArticulatedBodyInertia *result = 0 ;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkPositions_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_paren" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkPositions_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::ArticulatedBodyInertia *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyInertia, 0 |  0 );
+  result = (iDynTree::Transform *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42301,31 +41898,31 @@ int _wrap_LinkArticulatedBodyInertias_paren__SWIG_0(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+int _wrap_LinkPositions_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::ArticulatedBodyInertia *result = 0 ;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkPositions_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_paren" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkPositions_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::ArticulatedBodyInertia *) &((iDynTree::LinkArticulatedBodyInertias const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyInertia, 0 |  0 );
+  result = (iDynTree::Transform *) &((iDynTree::LinkPositions const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42333,11 +41930,11 @@ int _wrap_LinkArticulatedBodyInertias_paren__SWIG_1(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_LinkArticulatedBodyInertias_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_LinkPositions_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -42345,14 +41942,14 @@ int _wrap_LinkArticulatedBodyInertias_paren(int resc, mxArray *resv[], int argc,
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkArticulatedBodyInertias_paren__SWIG_0(resc,resv,argc,argv);
+        return _wrap_LinkPositions_paren__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -42360,35 +41957,70 @@ int _wrap_LinkArticulatedBodyInertias_paren(int resc, mxArray *resv[], int argc,
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkArticulatedBodyInertias_paren__SWIG_1(resc,resv,argc,argv);
+        return _wrap_LinkPositions_paren__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkArticulatedBodyInertias_paren'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkPositions_paren'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkArticulatedBodyInertias::operator ()(iDynTree::LinkIndex const)\n"
-    "    iDynTree::LinkArticulatedBodyInertias::operator ()(iDynTree::LinkIndex const) const\n");
+    "    iDynTree::LinkPositions::operator ()(iDynTree::LinkIndex const)\n"
+    "    iDynTree::LinkPositions::operator ()(iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_delete_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+int _wrap_LinkPositions_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  std::string result;
+  
+  if (!SWIG_check_num_args("LinkPositions_toString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkPositions_toString" "', argument " "1"" of type '" "iDynTree::LinkPositions const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkPositions_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkPositions_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = ((iDynTree::LinkPositions const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_LinkPositions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkPositions *arg1 = (iDynTree::LinkPositions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_LinkArticulatedBodyInertias",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_LinkPositions",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkPositions, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkPositions" "', argument " "1"" of type '" "iDynTree::LinkPositions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkPositions * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -42400,23 +42032,23 @@ int _wrap_delete_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_new_LinkVelArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkWrenches__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   unsigned int arg1 ;
   unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkVelArray *result = 0 ;
+  iDynTree::LinkWrenches *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkVelArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkWrenches",argc,1,1,0)) {
     SWIG_fail;
   }
   ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkVelArray" "', argument " "1"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkWrenches" "', argument " "1"" of type '" "unsigned int""'");
   } 
   arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkVelArray *)new iDynTree::LinkVelArray(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 1 |  0 );
+  result = (iDynTree::LinkWrenches *)new iDynTree::LinkWrenches(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42424,16 +42056,16 @@ int _wrap_new_LinkVelArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkVelArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkWrenches__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkVelArray *result = 0 ;
+  iDynTree::LinkWrenches *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkVelArray",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_LinkWrenches",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::LinkVelArray *)new iDynTree::LinkVelArray();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 1 |  0 );
+  result = (iDynTree::LinkWrenches *)new iDynTree::LinkWrenches();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42441,26 +42073,26 @@ int _wrap_new_LinkVelArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkVelArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkWrenches__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkVelArray *result = 0 ;
+  iDynTree::LinkWrenches *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkVelArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkWrenches",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkVelArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkVelArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkVelArray *)new iDynTree::LinkVelArray((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 1 |  0 );
+  result = (iDynTree::LinkWrenches *)new iDynTree::LinkWrenches((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42468,9 +42100,9 @@ int _wrap_new_LinkVelArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_LinkVelArray__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_LinkWrenches__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -42478,7 +42110,7 @@ int _wrap_new_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv[])
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_LinkVelArray__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_LinkWrenches__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
@@ -42488,21 +42120,21 @@ int _wrap_new_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv[])
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_LinkVelArray__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_LinkWrenches__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkVelArray'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkWrenches'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkVelArray::LinkVelArray(unsigned int)\n"
-    "    iDynTree::LinkVelArray::LinkVelArray()\n"
-    "    iDynTree::LinkVelArray::LinkVelArray(iDynTree::Model const &)\n");
+    "    iDynTree::LinkWrenches::LinkWrenches(unsigned int)\n"
+    "    iDynTree::LinkWrenches::LinkWrenches()\n"
+    "    iDynTree::LinkWrenches::LinkWrenches(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkVelArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   unsigned int arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42510,17 +42142,17 @@ int _wrap_LinkVelArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkVelArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_resize" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkVelArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkWrenches_resize" "', argument " "2"" of type '" "unsigned int""'");
   } 
   arg2 = static_cast< unsigned int >(val2);
   (arg1)->resize(arg2);
@@ -42532,8 +42164,8 @@ int _wrap_LinkVelArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkVelArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42541,20 +42173,20 @@ int _wrap_LinkVelArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkVelArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_resize" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkVelArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkVelArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -42566,25 +42198,25 @@ int _wrap_LinkVelArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkVelArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_LinkWrenches_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
       int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_LinkVelArray_resize__SWIG_1(resc,resv,argc,argv);
+        return _wrap_LinkWrenches_resize__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -42592,21 +42224,21 @@ int _wrap_LinkVelArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkVelArray_resize__SWIG_0(resc,resv,argc,argv);
+        return _wrap_LinkWrenches_resize__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkVelArray_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkWrenches_resize'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkVelArray::resize(unsigned int)\n"
-    "    iDynTree::LinkVelArray::resize(iDynTree::Model const &)\n");
+    "    iDynTree::LinkWrenches::resize(unsigned int)\n"
+    "    iDynTree::LinkWrenches::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkVelArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42615,23 +42247,23 @@ int _wrap_LinkVelArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("LinkVelArray_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkVelArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkWrenches_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkVelArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkWrenches_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::LinkVelArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  result = (bool)((iDynTree::LinkWrenches const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -42640,22 +42272,22 @@ int _wrap_LinkVelArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_LinkVelArray_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("LinkVelArray_getNrOfLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_getNrOfLinks",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
-  result = ((iDynTree::LinkVelArray const *)arg1)->getNrOfLinks();
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
+  result = ((iDynTree::LinkWrenches const *)arg1)->getNrOfLinks();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -42664,31 +42296,31 @@ int _wrap_LinkVelArray_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_LinkVelArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Twist *result = 0 ;
+  iDynTree::Wrench *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkVelArray_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_paren" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_paren" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkVelArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkWrenches_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Twist *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
+  result = (iDynTree::Wrench *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42696,31 +42328,31 @@ int _wrap_LinkVelArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkVelArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Twist *result = 0 ;
+  iDynTree::Wrench *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkVelArray_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_paren" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_paren" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkVelArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkWrenches_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Twist *) &((iDynTree::LinkVelArray const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
+  result = (iDynTree::Wrench *) &((iDynTree::LinkWrenches const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42728,11 +42360,11 @@ int _wrap_LinkVelArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkVelArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_LinkWrenches_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -42740,14 +42372,14 @@ int _wrap_LinkVelArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkVelArray_paren__SWIG_0(resc,resv,argc,argv);
+        return _wrap_LinkWrenches_paren__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkWrenches, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -42755,21 +42387,21 @@ int _wrap_LinkVelArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkVelArray_paren__SWIG_1(resc,resv,argc,argv);
+        return _wrap_LinkWrenches_paren__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkVelArray_paren'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkWrenches_paren'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkVelArray::operator ()(iDynTree::LinkIndex const)\n"
-    "    iDynTree::LinkVelArray::operator ()(iDynTree::LinkIndex const) const\n");
+    "    iDynTree::LinkWrenches::operator ()(iDynTree::LinkIndex const)\n"
+    "    iDynTree::LinkWrenches::operator ()(iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_LinkVelArray_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42778,23 +42410,23 @@ int _wrap_LinkVelArray_toString(int resc, mxArray *resv[], int argc, mxArray *ar
   mxArray * _out;
   std::string result;
   
-  if (!SWIG_check_num_args("LinkVelArray_toString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkWrenches_toString",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_toString" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_toString" "', argument " "1"" of type '" "iDynTree::LinkWrenches const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkVelArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkVelArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::LinkVelArray const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  result = ((iDynTree::LinkWrenches const *)arg1)->toString((iDynTree::Model const &)*arg2);
   _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -42803,22 +42435,45 @@ int _wrap_LinkVelArray_toString(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_delete_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+int _wrap_LinkWrenches_zero(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkWrenches_zero",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkWrenches_zero" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
+  (arg1)->zero();
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_LinkWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkWrenches *arg1 = (iDynTree::LinkWrenches *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_LinkVelArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_LinkWrenches",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkWrenches, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkVelArray" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkWrenches" "', argument " "1"" of type '" "iDynTree::LinkWrenches *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkWrenches * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -42830,23 +42485,23 @@ int _wrap_delete_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_new_LinkAccArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkInertias__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   unsigned int arg1 ;
   unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkAccArray *result = 0 ;
+  iDynTree::LinkInertias *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkAccArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkInertias",argc,1,1,0)) {
     SWIG_fail;
   }
   ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkAccArray" "', argument " "1"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkInertias" "', argument " "1"" of type '" "unsigned int""'");
   } 
   arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkAccArray *)new iDynTree::LinkAccArray(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 1 |  0 );
+  result = (iDynTree::LinkInertias *)new iDynTree::LinkInertias(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkInertias, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42854,16 +42509,16 @@ int _wrap_new_LinkAccArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkAccArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkInertias__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkAccArray *result = 0 ;
+  iDynTree::LinkInertias *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkAccArray",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_LinkInertias",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::LinkAccArray *)new iDynTree::LinkAccArray();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 1 |  0 );
+  result = (iDynTree::LinkInertias *)new iDynTree::LinkInertias();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkInertias, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42871,26 +42526,26 @@ int _wrap_new_LinkAccArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkAccArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkInertias__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkAccArray *result = 0 ;
+  iDynTree::LinkInertias *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkAccArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkInertias",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkAccArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkAccArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkAccArray *)new iDynTree::LinkAccArray((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 1 |  0 );
+  result = (iDynTree::LinkInertias *)new iDynTree::LinkInertias((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkInertias, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -42898,9 +42553,9 @@ int _wrap_new_LinkAccArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_LinkAccArray__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_LinkInertias__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -42908,7 +42563,7 @@ int _wrap_new_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv[])
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_LinkAccArray__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_LinkInertias__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
@@ -42918,21 +42573,21 @@ int _wrap_new_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv[])
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_LinkAccArray__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_LinkInertias__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkAccArray'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkInertias'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkAccArray::LinkAccArray(unsigned int)\n"
-    "    iDynTree::LinkAccArray::LinkAccArray()\n"
-    "    iDynTree::LinkAccArray::LinkAccArray(iDynTree::Model const &)\n");
+    "    iDynTree::LinkInertias::LinkInertias(unsigned int)\n"
+    "    iDynTree::LinkInertias::LinkInertias()\n"
+    "    iDynTree::LinkInertias::LinkInertias(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkAccArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+int _wrap_LinkInertias_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
   unsigned int arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42940,17 +42595,17 @@ int _wrap_LinkAccArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkAccArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkInertias_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_resize" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
   ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkAccArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkInertias_resize" "', argument " "2"" of type '" "unsigned int""'");
   } 
   arg2 = static_cast< unsigned int >(val2);
   (arg1)->resize(arg2);
@@ -42962,8 +42617,8 @@ int _wrap_LinkAccArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkAccArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+int _wrap_LinkInertias_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -42971,20 +42626,20 @@ int _wrap_LinkAccArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkAccArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkInertias_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_resize" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkAccArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkAccArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -42996,25 +42651,25 @@ int _wrap_LinkAccArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_LinkAccArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_LinkInertias_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
       int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_LinkAccArray_resize__SWIG_1(resc,resv,argc,argv);
+        return _wrap_LinkInertias_resize__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -43022,21 +42677,21 @@ int _wrap_LinkAccArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkAccArray_resize__SWIG_0(resc,resv,argc,argv);
+        return _wrap_LinkInertias_resize__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkAccArray_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkInertias_resize'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkAccArray::resize(unsigned int)\n"
-    "    iDynTree::LinkAccArray::resize(iDynTree::Model const &)\n");
+    "    iDynTree::LinkInertias::resize(unsigned int)\n"
+    "    iDynTree::LinkInertias::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkAccArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+int _wrap_LinkInertias_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -43045,23 +42700,23 @@ int _wrap_LinkAccArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("LinkAccArray_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkInertias_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkInertias const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkAccArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkAccArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::LinkAccArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  result = (bool)((iDynTree::LinkInertias const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -43070,31 +42725,31 @@ int _wrap_LinkAccArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_LinkAccArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+int _wrap_LinkInertias_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialAcc *result = 0 ;
+  iDynTree::SpatialInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkAccArray_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkInertias_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_paren" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkAccArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::SpatialAcc *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
+  result = (iDynTree::SpatialInertia *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43102,31 +42757,31 @@ int _wrap_LinkAccArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkAccArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+int _wrap_LinkInertias_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialAcc *result = 0 ;
+  iDynTree::SpatialInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkAccArray_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkInertias_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_paren" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkInertias const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkAccArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::SpatialAcc *) &((iDynTree::LinkAccArray const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
+  result = (iDynTree::SpatialInertia *) &((iDynTree::LinkInertias const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43134,11 +42789,11 @@ int _wrap_LinkAccArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_LinkAccArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_LinkInertias_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -43146,14 +42801,14 @@ int _wrap_LinkAccArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkAccArray_paren__SWIG_0(resc,resv,argc,argv);
+        return _wrap_LinkInertias_paren__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkInertias, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -43161,94 +42816,35 @@ int _wrap_LinkAccArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_LinkAccArray_paren__SWIG_1(resc,resv,argc,argv);
+        return _wrap_LinkInertias_paren__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkAccArray_paren'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkInertias_paren'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkAccArray::operator ()(iDynTree::LinkIndex const)\n"
-    "    iDynTree::LinkAccArray::operator ()(iDynTree::LinkIndex const) const\n");
-  return 1;
-}
-
-
-int _wrap_LinkAccArray_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  unsigned int result;
-  
-  if (!SWIG_check_num_args("LinkAccArray_getNrOfLinks",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
-  result = (unsigned int)((iDynTree::LinkAccArray const *)arg1)->getNrOfLinks();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_LinkAccArray_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  std::string result;
-  
-  if (!SWIG_check_num_args("LinkAccArray_toString",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_toString" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkAccArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkAccArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::LinkAccArray const *)arg1)->toString((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+    "    iDynTree::LinkInertias::operator ()(iDynTree::LinkIndex const)\n"
+    "    iDynTree::LinkInertias::operator ()(iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_delete_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+int _wrap_delete_LinkInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkInertias *arg1 = (iDynTree::LinkInertias *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_LinkAccArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_LinkInertias",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkInertias, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkAccArray" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkInertias" "', argument " "1"" of type '" "iDynTree::LinkInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkInertias * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -43260,16 +42856,23 @@ int _wrap_delete_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_new_Link(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkArticulatedBodyInertias__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::Link *result = 0 ;
+  iDynTree::LinkArticulatedBodyInertias *result = 0 ;
   
-  if (!SWIG_check_num_args("new_Link",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_LinkArticulatedBodyInertias",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::Link *)new iDynTree::Link();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 1 |  0 );
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "unsigned int""'");
+  } 
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::LinkArticulatedBodyInertias *)new iDynTree::LinkArticulatedBodyInertias(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43277,23 +42880,16 @@ int _wrap_new_Link(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Link_inertia__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_LinkArticulatedBodyInertias__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::SpatialInertia *result = 0 ;
+  iDynTree::LinkArticulatedBodyInertias *result = 0 ;
   
-  if (!SWIG_check_num_args("Link_inertia",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkArticulatedBodyInertias",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_inertia" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  result = (iDynTree::SpatialInertia *) &(arg1)->inertia();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
+  (void)argv;
+  result = (iDynTree::LinkArticulatedBodyInertias *)new iDynTree::LinkArticulatedBodyInertias();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43301,23 +42897,26 @@ int _wrap_Link_inertia__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_Link_inertia__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_LinkArticulatedBodyInertias__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialInertia *result = 0 ;
+  iDynTree::LinkArticulatedBodyInertias *result = 0 ;
   
-  if (!SWIG_check_num_args("Link_inertia",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkArticulatedBodyInertias",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_inertia" "', argument " "1"" of type '" "iDynTree::Link const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  result = (iDynTree::SpatialInertia *) &((iDynTree::Link const *)arg1)->inertia();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::LinkArticulatedBodyInertias *)new iDynTree::LinkArticulatedBodyInertias((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43325,60 +42924,62 @@ int _wrap_Link_inertia__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_Link_inertia(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_LinkArticulatedBodyInertias__SWIG_1(resc,resv,argc,argv);
+  }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Link, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_Link_inertia__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_LinkArticulatedBodyInertias__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Link, 0);
-    _v = SWIG_CheckState(res);
+    {
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
     if (_v) {
-      return _wrap_Link_inertia__SWIG_1(resc,resv,argc,argv);
+      return _wrap_new_LinkArticulatedBodyInertias__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Link_inertia'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkArticulatedBodyInertias'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Link::inertia()\n"
-    "    iDynTree::Link::inertia() const\n");
+    "    iDynTree::LinkArticulatedBodyInertias::LinkArticulatedBodyInertias(unsigned int)\n"
+    "    iDynTree::LinkArticulatedBodyInertias::LinkArticulatedBodyInertias()\n"
+    "    iDynTree::LinkArticulatedBodyInertias::LinkArticulatedBodyInertias(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_Link_setInertia(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
-  iDynTree::SpatialInertia *arg2 = 0 ;
-  void *argp1 = 0 ;
+int _wrap_LinkArticulatedBodyInertias_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("Link_setInertia",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_setInertia" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpatialInertia,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Link_setInertia" "', argument " "2"" of type '" "iDynTree::SpatialInertia &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Link_setInertia" "', argument " "2"" of type '" "iDynTree::SpatialInertia &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SpatialInertia * >(argp2);
-  (arg1)->setInertia(*arg2);
+  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -43387,23 +42988,33 @@ int _wrap_Link_setInertia(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_Link_getInertia(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
+int _wrap_LinkArticulatedBodyInertias_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("Link_getInertia",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_getInertia" "', argument " "1"" of type '" "iDynTree::Link const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  result = (iDynTree::SpatialInertia *) &((iDynTree::Link const *)arg1)->getInertia();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkArticulatedBodyInertias_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43411,33 +43022,73 @@ int _wrap_Link_getInertia(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_Link_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
-  iDynTree::LinkIndex *arg2 = 0 ;
+int _wrap_LinkArticulatedBodyInertias_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_LinkArticulatedBodyInertias_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkArticulatedBodyInertias_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkArticulatedBodyInertias_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkArticulatedBodyInertias::resize(unsigned int)\n"
+    "    iDynTree::LinkArticulatedBodyInertias::resize(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkArticulatedBodyInertias_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("Link_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_setIndex" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Link_setIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkArticulatedBodyInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Link_setIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkArticulatedBodyInertias_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkIndex * >(argp2);
-  (arg1)->setIndex(*arg2);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::LinkArticulatedBodyInertias const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43445,23 +43096,31 @@ int _wrap_Link_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Link_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
+int _wrap_LinkArticulatedBodyInertias_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  iDynTree::ArticulatedBodyInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("Link_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_getIndex" "', argument " "1"" of type '" "iDynTree::Link const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::Link const *)arg1)->getIndex();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::ArticulatedBodyInertia *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43469,26 +43128,31 @@ int _wrap_Link_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_Link(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
+int _wrap_LinkArticulatedBodyInertias_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::ArticulatedBodyInertia *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_Link",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkArticulatedBodyInertias_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Link" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkArticulatedBodyInertias_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::ArticulatedBodyInertia *) &((iDynTree::LinkArticulatedBodyInertias const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43496,22 +43160,62 @@ int _wrap_delete_Link(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_IJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_LinkArticulatedBodyInertias_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkArticulatedBodyInertias_paren__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkArticulatedBodyInertias_paren__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkArticulatedBodyInertias_paren'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkArticulatedBodyInertias::operator ()(iDynTree::LinkIndex const)\n"
+    "    iDynTree::LinkArticulatedBodyInertias::operator ()(iDynTree::LinkIndex const) const\n");
+  return 1;
+}
+
+
+int _wrap_delete_LinkArticulatedBodyInertias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkArticulatedBodyInertias *arg1 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_IJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_LinkArticulatedBodyInertias",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_IJoint" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkArticulatedBodyInertias" "', argument " "1"" of type '" "iDynTree::LinkArticulatedBodyInertias *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkArticulatedBodyInertias * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -43523,23 +43227,23 @@ int _wrap_delete_IJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_IJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_LinkVelArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::IJoint *result = 0 ;
+  iDynTree::LinkVelArray *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkVelArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_clone" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (iDynTree::IJoint *)((iDynTree::IJoint const *)arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkVelArray" "', argument " "1"" of type '" "unsigned int""'");
+  } 
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::LinkVelArray *)new iDynTree::LinkVelArray(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43547,23 +43251,16 @@ int _wrap_IJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_IJoint_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_LinkVelArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  unsigned int result;
+  iDynTree::LinkVelArray *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkVelArray",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (unsigned int)((iDynTree::IJoint const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  (void)argv;
+  result = (iDynTree::LinkVelArray *)new iDynTree::LinkVelArray();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43571,23 +43268,26 @@ int _wrap_IJoint_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_IJoint_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_LinkVelArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  iDynTree::LinkVelArray *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_LinkVelArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkVelArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (unsigned int)((iDynTree::IJoint const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkVelArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::LinkVelArray *)new iDynTree::LinkVelArray((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43595,37 +43295,62 @@ int _wrap_IJoint_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_IJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_new_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_LinkVelArray__SWIG_1(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_LinkVelArray__SWIG_2(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    {
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      return _wrap_new_LinkVelArray__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkVelArray'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkVelArray::LinkVelArray(unsigned int)\n"
+    "    iDynTree::LinkVelArray::LinkVelArray()\n"
+    "    iDynTree::LinkVelArray::LinkVelArray(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkVelArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+  unsigned int arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  unsigned int val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("IJoint_setAttachedLinks",argc,3,3,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_resize" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkVelArray_resize" "', argument " "2"" of type '" "unsigned int""'");
   } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  (arg1)->setAttachedLinks(arg2,arg3);
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -43634,32 +43359,32 @@ int _wrap_IJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_IJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::Transform *arg2 = 0 ;
+int _wrap_LinkVelArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("IJoint_setRestTransform",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_resize" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkVelArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkVelArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
-  (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -43668,23 +43393,73 @@ int _wrap_IJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_IJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_LinkVelArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_LinkVelArray_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkVelArray_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkVelArray_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkVelArray::resize(unsigned int)\n"
+    "    iDynTree::LinkVelArray::resize(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkVelArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  bool result;
   
-  if (!SWIG_check_num_args("IJoint_getFirstAttachedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::IJoint const *)arg1)->getFirstAttachedLink();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkVelArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkVelArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::LinkVelArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43692,23 +43467,23 @@ int _wrap_IJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_IJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_LinkVelArray_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  size_t result;
   
-  if (!SWIG_check_num_args("IJoint_getSecondAttachedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_getNrOfLinks",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::IJoint const *)arg1)->getSecondAttachedLink();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  result = ((iDynTree::LinkVelArray const *)arg1)->getNrOfLinks();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43716,39 +43491,31 @@ int _wrap_IJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_IJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_LinkVelArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
   iDynTree::LinkIndex arg2 ;
-  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::Transform result;
+  iDynTree::Twist *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_getRestTransform",argc,3,3,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_paren" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkVelArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::IJoint const *)arg1)->getRestTransform(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  result = (iDynTree::Twist *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43756,50 +43523,31 @@ int _wrap_IJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_IJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
+int _wrap_LinkVelArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
+  iDynTree::Twist *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_getTransform",argc,4,4,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getTransform" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_paren" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkVelArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = (iDynTree::Transform *) &((iDynTree::IJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::Twist *) &((iDynTree::LinkVelArray const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43807,58 +43555,74 @@ int _wrap_IJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_IJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
-  int arg5 ;
+int _wrap_LinkVelArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkVelArray_paren__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkVelArray_paren__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkVelArray_paren'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkVelArray::operator ()(iDynTree::LinkIndex const)\n"
+    "    iDynTree::LinkVelArray::operator ()(iDynTree::LinkIndex const) const\n");
+  return 1;
+}
+
+
+int _wrap_LinkVelArray_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  int val5 ;
-  int ecode5 = 0 ;
   mxArray * _out;
-  iDynTree::TransformDerivative result;
+  std::string result;
   
-  if (!SWIG_check_num_args("IJoint_getTransformDerivative",argc,5,5,0)) {
+  if (!SWIG_check_num_args("LinkVelArray_toString",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkVelArray_toString" "', argument " "1"" of type '" "iDynTree::LinkVelArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkVelArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkVelArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
-  } 
-  arg5 = static_cast< int >(val5);
-  result = ((iDynTree::IJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
-  _out = SWIG_NewPointerObj((new iDynTree::TransformDerivative(static_cast< const iDynTree::TransformDerivative& >(result))), SWIGTYPE_p_iDynTree__TransformDerivative, SWIG_POINTER_OWN |  0 );
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = ((iDynTree::LinkVelArray const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43866,47 +43630,26 @@ int _wrap_IJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_IJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  int arg2 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
+int _wrap_delete_LinkVelArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkVelArray *arg1 = (iDynTree::LinkVelArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("IJoint_getMotionSubspaceVector",argc,4,4,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_LinkVelArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkVelArray, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkVelArray" "', argument " "1"" of type '" "iDynTree::LinkVelArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
-  } 
-  arg2 = static_cast< int >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = ((iDynTree::IJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
-  _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkVelArray * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -43914,104 +43657,23 @@ int _wrap_IJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_IJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
-  iDynTree::LinkPositions *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  iDynTree::LinkAccArray *arg7 = 0 ;
-  iDynTree::LinkIndex arg8 ;
-  iDynTree::LinkIndex arg9 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
-  int val8 ;
-  int ecode8 = 0 ;
-  int val9 ;
-  int ecode9 = 0 ;
+int _wrap_new_LinkAccArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
+  iDynTree::LinkAccArray *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_computeChildPosVelAcc",argc,9,9,0)) {
+  if (!SWIG_check_num_args("new_LinkAccArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
-  ecode8 = SWIG_AsVal_int(argv[7], &val8);
-  if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ecode9 = SWIG_AsVal_int(argv[8], &val9);
-  if (!SWIG_IsOK(ecode9)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkAccArray" "', argument " "1"" of type '" "unsigned int""'");
   } 
-  arg9 = static_cast< iDynTree::LinkIndex >(val9);
-  ((iDynTree::IJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
-  _out = (mxArray*)0;
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::LinkAccArray *)new iDynTree::LinkAccArray(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44019,92 +43681,106 @@ int _wrap_IJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_IJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
-  iDynTree::LinkVelArray *arg5 = 0 ;
-  iDynTree::LinkAccArray *arg6 = 0 ;
-  iDynTree::LinkIndex arg7 ;
-  iDynTree::LinkIndex arg8 ;
-  void *argp1 = 0 ;
+int _wrap_new_LinkAccArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::LinkAccArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_LinkAccArray",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::LinkAccArray *)new iDynTree::LinkAccArray();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkAccArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  int val7 ;
-  int ecode7 = 0 ;
-  int val8 ;
-  int ecode8 = 0 ;
   mxArray * _out;
+  iDynTree::LinkAccArray *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_computeChildVelAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("new_LinkAccArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkAccArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkAccArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::LinkAccArray *)new iDynTree::LinkAccArray((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_LinkAccArray__SWIG_1(resc,resv,argc,argv);
   }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_LinkAccArray__SWIG_2(resc,resv,argc,argv);
+    }
   }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+  if (argc == 1) {
+    int _v;
+    {
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      return _wrap_new_LinkAccArray__SWIG_0(resc,resv,argc,argv);
+    }
   }
-  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkAccArray'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkAccArray::LinkAccArray(unsigned int)\n"
+    "    iDynTree::LinkAccArray::LinkAccArray()\n"
+    "    iDynTree::LinkAccArray::LinkAccArray(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkAccArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkAccArray_resize",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_resize" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
-  ecode7 = SWIG_AsVal_int(argv[6], &val7);
-  if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "IJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ecode8 = SWIG_AsVal_int(argv[7], &val8);
-  if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "IJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkAccArray_resize" "', argument " "2"" of type '" "unsigned int""'");
   } 
-  arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::IJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -44113,70 +43789,32 @@ int _wrap_IJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_IJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::LinkIndex arg5 ;
-  iDynTree::LinkIndex arg6 ;
+int _wrap_LinkAccArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  int val5 ;
-  int ecode5 = 0 ;
-  int val6 ;
-  int ecode6 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("IJoint_computeChildVel",argc,6,6,0)) {
+  if (!SWIG_check_num_args("LinkAccArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_resize" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkAccArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkAccArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg5 = static_cast< iDynTree::LinkIndex >(val5);
-  ecode6 = SWIG_AsVal_int(argv[5], &val6);
-  if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "IJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg6 = static_cast< iDynTree::LinkIndex >(val6);
-  ((iDynTree::IJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -44185,176 +43823,73 @@ int _wrap_IJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_IJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::VectorDynSize *arg5 = 0 ;
-  iDynTree::LinkAccArray *arg6 = 0 ;
-  iDynTree::LinkIndex arg7 ;
-  iDynTree::LinkIndex arg8 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  int val7 ;
-  int ecode7 = 0 ;
-  int val8 ;
-  int ecode8 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("IJoint_computeChildAcc",argc,8,8,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+int _wrap_LinkAccArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_LinkAccArray_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
   }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkAccArray_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
   }
-  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
-  ecode7 = SWIG_AsVal_int(argv[6], &val7);
-  if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "IJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ecode8 = SWIG_AsVal_int(argv[7], &val8);
-  if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "IJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::IJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkAccArray_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkAccArray::resize(unsigned int)\n"
+    "    iDynTree::LinkAccArray::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_IJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::LinkAccArray *arg5 = 0 ;
-  iDynTree::LinkIndex arg6 ;
-  iDynTree::LinkIndex arg7 ;
+int _wrap_LinkAccArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  int val6 ;
-  int ecode6 = 0 ;
-  int val7 ;
-  int ecode7 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("IJoint_computeChildBiasAcc",argc,7,7,0)) {
+  if (!SWIG_check_num_args("LinkAccArray_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_isConsistent" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkAccArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkAccArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
-  ecode6 = SWIG_AsVal_int(argv[5], &val6);
-  if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "IJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg6 = static_cast< iDynTree::LinkIndex >(val6);
-  ecode7 = SWIG_AsVal_int(argv[6], &val7);
-  if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "IJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ((iDynTree::IJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::LinkAccArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44362,71 +43897,31 @@ int _wrap_IJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_IJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::Wrench *arg3 = 0 ;
-  iDynTree::LinkIndex arg4 ;
-  iDynTree::LinkIndex arg5 ;
-  iDynTree::VectorDynSize *arg6 = 0 ;
+int _wrap_LinkAccArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  int val5 ;
-  int ecode5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::SpatialAcc *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_computeJointTorque",argc,6,6,0)) {
+  if (!SWIG_check_num_args("LinkAccArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Wrench,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_paren" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Wrench * >(argp3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkAccArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg5 = static_cast< iDynTree::LinkIndex >(val5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
-  ((iDynTree::IJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
-  _out = (mxArray*)0;
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::SpatialAcc *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44434,33 +43929,31 @@ int _wrap_IJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_IJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  iDynTree::JointIndex *arg2 = 0 ;
+int _wrap_LinkAccArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::SpatialAcc *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkAccArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setIndex" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_paren" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
-  (arg1)->setIndex(*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkAccArray_paren" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::SpatialAcc *) &((iDynTree::LinkAccArray const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44468,54 +43961,63 @@ int _wrap_IJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_IJoint_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::JointIndex result;
-  
-  if (!SWIG_check_num_args("IJoint_getIndex",argc,1,1,0)) {
-    SWIG_fail;
+int _wrap_LinkAccArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkAccArray_paren__SWIG_0(resc,resv,argc,argv);
+      }
+    }
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getIndex" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkAccArray_paren__SWIG_1(resc,resv,argc,argv);
+      }
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::IJoint const *)arg1)->getIndex();
-  _out = SWIG_From_int(static_cast< int >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkAccArray_paren'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkAccArray::operator ()(iDynTree::LinkIndex const)\n"
+    "    iDynTree::LinkAccArray::operator ()(iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_IJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  size_t arg2 ;
+int _wrap_LinkAccArray_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("IJoint_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("LinkAccArray_getNrOfLinks",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setPosCoordsOffset(arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  result = (unsigned int)((iDynTree::LinkAccArray const *)arg1)->getNrOfLinks();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44523,23 +44025,34 @@ int _wrap_IJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_IJoint_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_LinkAccArray_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  size_t result;
+  std::string result;
   
-  if (!SWIG_check_num_args("IJoint_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("LinkAccArray_toString",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkAccArray_toString" "', argument " "1"" of type '" "iDynTree::LinkAccArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = ((iDynTree::IJoint const *)arg1)->getPosCoordsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkAccArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkAccArray_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = ((iDynTree::LinkAccArray const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44547,29 +44060,25 @@ int _wrap_IJoint_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_IJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  size_t arg2 ;
+int _wrap_delete_LinkAccArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkAccArray *arg1 = (iDynTree::LinkAccArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("IJoint_setDOFsOffset",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_LinkAccArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkAccArray, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkAccArray" "', argument " "1"" of type '" "iDynTree::LinkAccArray *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkAccArray * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setDOFsOffset(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -44578,23 +44087,16 @@ int _wrap_IJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_IJoint_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_Link(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  size_t result;
+  iDynTree::Link *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_Link",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = ((iDynTree::IJoint const *)arg1)->getDOFsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  (void)argv;
+  result = (iDynTree::Link *)new iDynTree::Link();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44602,23 +44104,23 @@ int _wrap_IJoint_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_IJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_Link_inertia__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::SpatialInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_hasPosLimits",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Link_inertia",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_inertia" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (bool)((iDynTree::IJoint const *)arg1)->hasPosLimits();
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  result = (iDynTree::SpatialInertia *) &(arg1)->inertia();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44626,31 +44128,23 @@ int _wrap_IJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_IJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  bool arg2 ;
+int _wrap_Link_inertia__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  bool val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::SpatialInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_enablePosLimits",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Link_inertia",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_inertia" "', argument " "1"" of type '" "iDynTree::Link const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
-  } 
-  arg2 = static_cast< bool >(val2);
-  result = (bool)(arg1)->enablePosLimits(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  result = (iDynTree::SpatialInertia *) &((iDynTree::Link const *)arg1)->inertia();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44658,85 +44152,61 @@ int _wrap_IJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_IJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  size_t arg2 ;
-  double *arg3 = 0 ;
-  double *arg4 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("IJoint_getPosLimits",argc,4,4,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  arg3 = reinterpret_cast< double * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+int _wrap_Link_inertia(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Link, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_Link_inertia__SWIG_0(resc,resv,argc,argv);
+    }
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Link, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_Link_inertia__SWIG_1(resc,resv,argc,argv);
+    }
   }
-  arg4 = reinterpret_cast< double * >(argp4);
-  result = (bool)((iDynTree::IJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Link_inertia'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::Link::inertia()\n"
+    "    iDynTree::Link::inertia() const\n");
   return 1;
 }
 
 
-int _wrap_IJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  size_t arg2 ;
+int _wrap_Link_setInertia(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
+  iDynTree::SpatialInertia *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  double result;
   
-  if (!SWIG_check_num_args("IJoint_getMinPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Link_setInertia",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_setInertia" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::IJoint const *)arg1)->getMinPosLimit(arg2);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpatialInertia,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Link_setInertia" "', argument " "2"" of type '" "iDynTree::SpatialInertia &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Link_setInertia" "', argument " "2"" of type '" "iDynTree::SpatialInertia &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SpatialInertia * >(argp2);
+  (arg1)->setInertia(*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44744,31 +44214,23 @@ int _wrap_IJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_IJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  size_t arg2 ;
+int _wrap_Link_getInertia(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  double result;
+  iDynTree::SpatialInertia *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_getMaxPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Link_getInertia",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_getInertia" "', argument " "1"" of type '" "iDynTree::Link const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::IJoint const *)arg1)->getMaxPosLimit(arg2);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  result = (iDynTree::SpatialInertia *) &((iDynTree::Link const *)arg1)->getInertia();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialInertia, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44776,53 +44238,33 @@ int _wrap_IJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_IJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
-  size_t arg2 ;
-  double *arg3 = 0 ;
-  double *arg4 = 0 ;
+int _wrap_Link_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
+  iDynTree::LinkIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("IJoint_setPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("Link_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_setIndex" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
   }
-  arg3 = reinterpret_cast< double * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Link_setIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex &""'"); 
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Link_setIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex &""'"); 
   }
-  arg4 = reinterpret_cast< double * >(argp4);
-  result = (bool)(arg1)->setPosLimits(arg2,*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::LinkIndex * >(argp2);
+  (arg1)->setIndex(*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44830,23 +44272,23 @@ int _wrap_IJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_IJoint_isRevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_Link_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("IJoint_isRevoluteJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Link_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_isRevoluteJoint" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Link_getIndex" "', argument " "1"" of type '" "iDynTree::Link const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (bool)iDynTree_IJoint_isRevoluteJoint((iDynTree::IJoint const *)arg1);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::Link const *)arg1)->getIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44854,23 +44296,26 @@ int _wrap_IJoint_isRevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_IJoint_isFixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+int _wrap_delete_Link(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Link *arg1 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("IJoint_isFixedJoint",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_Link",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Link, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_isFixedJoint" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Link" "', argument " "1"" of type '" "iDynTree::Link *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (bool)iDynTree_IJoint_isFixedJoint((iDynTree::IJoint const *)arg1);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::Link * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44878,23 +44323,26 @@ int _wrap_IJoint_isFixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_IJoint_asRevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_delete_IJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_asRevoluteJoint",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_IJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_asRevoluteJoint" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_IJoint" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (iDynTree::RevoluteJoint *)iDynTree_IJoint_asRevoluteJoint(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44902,66 +44350,23 @@ int _wrap_IJoint_asRevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_IJoint_asFixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_IJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FixedJoint *result = 0 ;
+  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("IJoint_asFixedJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_clone",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_asFixedJoint" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_clone" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
-  result = (iDynTree::FixedJoint *)iDynTree_IJoint_asFixedJoint(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_FixedJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkIndex arg1 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::Transform *arg3 = 0 ;
-  int val1 ;
-  int ecode1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  mxArray * _out;
-  iDynTree::FixedJoint *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_FixedJoint",argc,3,3,0)) {
-    SWIG_fail;
-  }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg1 = static_cast< iDynTree::LinkIndex >(val1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_FixedJoint" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_FixedJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FixedJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
-  result = (iDynTree::FixedJoint *)new iDynTree::FixedJoint(arg1,arg2,(iDynTree::Transform const &)*arg3);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 1 |  0 );
+  result = (iDynTree::IJoint *)((iDynTree::IJoint const *)arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44969,26 +44374,23 @@ int _wrap_new_FixedJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_new_FixedJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Transform *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_IJoint_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FixedJoint *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("new_FixedJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Transform,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Transform * >(argp1);
-  result = (iDynTree::FixedJoint *)new iDynTree::FixedJoint((iDynTree::Transform const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (unsigned int)((iDynTree::IJoint const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -44996,26 +44398,23 @@ int _wrap_new_FixedJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_new_FixedJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_IJoint_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FixedJoint *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("new_FixedJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__FixedJoint,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::FixedJoint const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::FixedJoint const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (iDynTree::FixedJoint *)new iDynTree::FixedJoint((iDynTree::FixedJoint const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (unsigned int)((iDynTree::IJoint const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -45023,75 +44422,37 @@ int _wrap_new_FixedJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_new_FixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_FixedJoint__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FixedJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_FixedJoint__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 3) {
-    int _v;
-    {
-      int res = SWIG_AsVal_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_new_FixedJoint__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FixedJoint'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FixedJoint::FixedJoint(iDynTree::LinkIndex const,iDynTree::LinkIndex const,iDynTree::Transform const &)\n"
-    "    iDynTree::FixedJoint::FixedJoint(iDynTree::Transform const &)\n"
-    "    iDynTree::FixedJoint::FixedJoint(iDynTree::FixedJoint const &)\n");
-  return 1;
-}
-
-
-int _wrap_delete_FixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_FixedJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_setAttachedLinks",argc,3,3,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FixedJoint" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  (arg1)->setAttachedLinks(arg2,arg3);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45100,23 +44461,33 @@ int _wrap_delete_FixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_FixedJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+  iDynTree::Transform *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("FixedJoint_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_setRestTransform",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_clone" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (iDynTree::IJoint *)((iDynTree::FixedJoint const *)arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
+  (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -45124,23 +44495,23 @@ int _wrap_FixedJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_FixedJoint_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("FixedJoint_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getFirstAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (unsigned int)((iDynTree::FixedJoint const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::IJoint const *)arg1)->getFirstAttachedLink();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -45148,23 +44519,23 @@ int _wrap_FixedJoint_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_FixedJoint_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("FixedJoint_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getSecondAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (unsigned int)((iDynTree::FixedJoint const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::IJoint const *)arg1)->getSecondAttachedLink();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -45172,8 +44543,8 @@ int _wrap_FixedJoint_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_FixedJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::LinkIndex arg2 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -45183,27 +44554,28 @@ int _wrap_FixedJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArr
   int val3 ;
   int ecode3 = 0 ;
   mxArray * _out;
+  iDynTree::Transform result;
   
-  if (!SWIG_check_num_args("FixedJoint_setAttachedLinks",argc,3,3,0)) {
+  if (!SWIG_check_num_args("IJoint_getRestTransform",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  (arg1)->setAttachedLinks(arg2,arg3);
-  _out = (mxArray*)0;
+  result = ((iDynTree::IJoint const *)arg1)->getRestTransform(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -45211,171 +44583,49 @@ int _wrap_FixedJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_FixedJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
-  iDynTree::Transform *arg2 = 0 ;
+int _wrap_IJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("FixedJoint_setRestTransform",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
-  (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_FixedJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::LinkIndex result;
-  
-  if (!SWIG_check_num_args("FixedJoint_getFirstAttachedLink",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::FixedJoint const *)arg1)->getFirstAttachedLink();
-  _out = SWIG_From_int(static_cast< int >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_FixedJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::LinkIndex result;
-  
-  if (!SWIG_check_num_args("FixedJoint_getSecondAttachedLink",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::FixedJoint const *)arg1)->getSecondAttachedLink();
-  _out = SWIG_From_int(static_cast< int >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_FixedJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::LinkIndex arg3 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  mxArray * _out;
-  iDynTree::Transform result;
-  
-  if (!SWIG_check_num_args("FixedJoint_getRestTransform",argc,3,3,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::FixedJoint const *)arg1)->getRestTransform(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_FixedJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  mxArray * _out;
-  iDynTree::Transform *result = 0 ;
-  
-  if (!SWIG_check_num_args("FixedJoint_getTransform",argc,4,4,0)) {
+  if (!SWIG_check_num_args("IJoint_getTransform",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getTransform" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getTransform" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = (iDynTree::Transform *) &((iDynTree::FixedJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
+  result = (iDynTree::Transform *) &((iDynTree::IJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
   _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45384,8 +44634,8 @@ int _wrap_FixedJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_FixedJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::LinkIndex arg3 ;
   iDynTree::LinkIndex arg4 ;
@@ -45403,38 +44653,38 @@ int _wrap_FixedJoint_getTransformDerivative(int resc, mxArray *resv[], int argc,
   mxArray * _out;
   iDynTree::TransformDerivative result;
   
-  if (!SWIG_check_num_args("FixedJoint_getTransformDerivative",argc,5,5,0)) {
+  if (!SWIG_check_num_args("IJoint_getTransformDerivative",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
   ecode5 = SWIG_AsVal_int(argv[4], &val5);
   if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "FixedJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
   } 
   arg5 = static_cast< int >(val5);
-  result = ((iDynTree::FixedJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
+  result = ((iDynTree::IJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
   _out = SWIG_NewPointerObj((new iDynTree::TransformDerivative(static_cast< const iDynTree::TransformDerivative& >(result))), SWIGTYPE_p_iDynTree__TransformDerivative, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45443,8 +44693,8 @@ int _wrap_FixedJoint_getTransformDerivative(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FixedJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   int arg2 ;
   iDynTree::LinkIndex arg3 ;
   iDynTree::LinkIndex arg4 ;
@@ -45459,30 +44709,30 @@ int _wrap_FixedJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc
   mxArray * _out;
   iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("FixedJoint_getMotionSubspaceVector",argc,4,4,0)) {
+  if (!SWIG_check_num_args("IJoint_getMotionSubspaceVector",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
   } 
   arg2 = static_cast< int >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = ((iDynTree::FixedJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
+  result = ((iDynTree::IJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
   _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45491,8 +44741,8 @@ int _wrap_FixedJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_FixedJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::VectorDynSize *arg4 = 0 ;
@@ -45521,73 +44771,73 @@ int _wrap_FixedJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc,
   int ecode9 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_computeChildPosVelAcc",argc,9,9,0)) {
+  if (!SWIG_check_num_args("IJoint_computeChildPosVelAcc",argc,9,9,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
   res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
   ecode8 = SWIG_AsVal_int(argv[7], &val8);
   if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg8 = static_cast< iDynTree::LinkIndex >(val8);
   ecode9 = SWIG_AsVal_int(argv[8], &val9);
   if (!SWIG_IsOK(ecode9)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "IJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg9 = static_cast< iDynTree::LinkIndex >(val9);
-  ((iDynTree::FixedJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
+  ((iDynTree::IJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45596,8 +44846,8 @@ int _wrap_FixedJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FixedJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::VectorDynSize *arg4 = 0 ;
@@ -45623,65 +44873,65 @@ int _wrap_FixedJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxA
   int ecode8 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_computeChildVelAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("IJoint_computeChildVelAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
   ecode7 = SWIG_AsVal_int(argv[6], &val7);
   if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "IJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg7 = static_cast< iDynTree::LinkIndex >(val7);
   ecode8 = SWIG_AsVal_int(argv[7], &val8);
   if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "IJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::FixedJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
+  ((iDynTree::IJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45690,8 +44940,8 @@ int _wrap_FixedJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_FixedJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::LinkVelArray *arg4 = 0 ;
@@ -45711,49 +44961,49 @@ int _wrap_FixedJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArra
   int ecode6 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_computeChildVel",argc,6,6,0)) {
+  if (!SWIG_check_num_args("IJoint_computeChildVel",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
   ecode5 = SWIG_AsVal_int(argv[4], &val5);
   if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "FixedJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg5 = static_cast< iDynTree::LinkIndex >(val5);
   ecode6 = SWIG_AsVal_int(argv[5], &val6);
   if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "FixedJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "IJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg6 = static_cast< iDynTree::LinkIndex >(val6);
-  ((iDynTree::FixedJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
+  ((iDynTree::IJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45762,8 +45012,8 @@ int _wrap_FixedJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_FixedJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::LinkVelArray *arg4 = 0 ;
@@ -45789,65 +45039,65 @@ int _wrap_FixedJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArra
   int ecode8 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_computeChildAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("IJoint_computeChildAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
   ecode7 = SWIG_AsVal_int(argv[6], &val7);
   if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "FixedJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "IJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg7 = static_cast< iDynTree::LinkIndex >(val7);
   ecode8 = SWIG_AsVal_int(argv[7], &val8);
   if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "FixedJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "IJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::FixedJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
+  ((iDynTree::IJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45856,8 +45106,8 @@ int _wrap_FixedJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_FixedJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::LinkVelArray *arg4 = 0 ;
@@ -45880,57 +45130,57 @@ int _wrap_FixedJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mx
   int ecode7 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_computeChildBiasAcc",argc,7,7,0)) {
+  if (!SWIG_check_num_args("IJoint_computeChildBiasAcc",argc,7,7,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
   ecode6 = SWIG_AsVal_int(argv[5], &val6);
   if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "IJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg6 = static_cast< iDynTree::LinkIndex >(val6);
   ecode7 = SWIG_AsVal_int(argv[6], &val7);
   if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "IJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ((iDynTree::FixedJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
+  ((iDynTree::IJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -45939,8 +45189,8 @@ int _wrap_FixedJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_FixedJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::Wrench *arg3 = 0 ;
   iDynTree::LinkIndex arg4 ;
@@ -45960,49 +45210,49 @@ int _wrap_FixedJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxA
   int res6 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_computeJointTorque",argc,6,6,0)) {
+  if (!SWIG_check_num_args("IJoint_computeJointTorque",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Wrench,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::Wrench * >(argp3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
   ecode5 = SWIG_AsVal_int(argv[4], &val5);
   if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "FixedJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg5 = static_cast< iDynTree::LinkIndex >(val5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "IJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
-  ((iDynTree::FixedJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
+  ((iDynTree::IJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46011,8 +45261,8 @@ int _wrap_FixedJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_FixedJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -46020,20 +45270,20 @@ int _wrap_FixedJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setIndex" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setIndex" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
   (arg1)->setIndex(*arg2);
@@ -46045,22 +45295,22 @@ int _wrap_FixedJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_FixedJoint_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("FixedJoint_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getIndex" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getIndex" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::FixedJoint const *)arg1)->getIndex();
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::IJoint const *)arg1)->getIndex();
   _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46069,8 +45319,8 @@ int _wrap_FixedJoint_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_FixedJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -46078,17 +45328,17 @@ int _wrap_FixedJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxA
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   (arg1)->setPosCoordsOffset(arg2);
@@ -46100,22 +45350,22 @@ int _wrap_FixedJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_FixedJoint_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("FixedJoint_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = ((iDynTree::FixedJoint const *)arg1)->getPosCoordsOffset();
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = ((iDynTree::IJoint const *)arg1)->getPosCoordsOffset();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46124,8 +45374,8 @@ int _wrap_FixedJoint_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_FixedJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -46133,17 +45383,17 @@ int _wrap_FixedJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FixedJoint_setDOFsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   (arg1)->setDOFsOffset(arg2);
@@ -46155,22 +45405,22 @@ int _wrap_FixedJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_FixedJoint_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("FixedJoint_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = ((iDynTree::FixedJoint const *)arg1)->getDOFsOffset();
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = ((iDynTree::IJoint const *)arg1)->getDOFsOffset();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46179,22 +45429,22 @@ int _wrap_FixedJoint_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_FixedJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("FixedJoint_hasPosLimits",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_hasPosLimits",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
-  result = (bool)((iDynTree::FixedJoint const *)arg1)->hasPosLimits();
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (bool)((iDynTree::IJoint const *)arg1)->hasPosLimits();
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46203,8 +45453,8 @@ int _wrap_FixedJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_FixedJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -46213,17 +45463,17 @@ int _wrap_FixedJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArra
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("FixedJoint_enablePosLimits",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_enablePosLimits",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_bool(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
   } 
   arg2 = static_cast< bool >(val2);
   result = (bool)(arg1)->enablePosLimits(arg2);
@@ -46235,8 +45485,8 @@ int _wrap_FixedJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_FixedJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   size_t arg2 ;
   double *arg3 = 0 ;
   double *arg4 = 0 ;
@@ -46251,36 +45501,36 @@ int _wrap_FixedJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("FixedJoint_getPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("IJoint_getPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   arg3 = reinterpret_cast< double * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   arg4 = reinterpret_cast< double * >(argp4);
-  result = (bool)((iDynTree::FixedJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
+  result = (bool)((iDynTree::IJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46289,8 +45539,8 @@ int _wrap_FixedJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_FixedJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -46299,20 +45549,20 @@ int _wrap_FixedJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray
   mxArray * _out;
   double result;
   
-  if (!SWIG_check_num_args("FixedJoint_getMinPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_getMinPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::FixedJoint const *)arg1)->getMinPosLimit(arg2);
+  result = (double)((iDynTree::IJoint const *)arg1)->getMinPosLimit(arg2);
   _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46321,8 +45571,8 @@ int _wrap_FixedJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_FixedJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -46331,20 +45581,20 @@ int _wrap_FixedJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray
   mxArray * _out;
   double result;
   
-  if (!SWIG_check_num_args("FixedJoint_getMaxPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_getMaxPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::FixedJoint const *)arg1)->getMaxPosLimit(arg2);
+  result = (double)((iDynTree::IJoint const *)arg1)->getMaxPosLimit(arg2);
   _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46353,8 +45603,8 @@ int _wrap_FixedJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_FixedJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+int _wrap_IJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   size_t arg2 ;
   double *arg3 = 0 ;
   double *arg4 = 0 ;
@@ -46369,33 +45619,33 @@ int _wrap_FixedJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("FixedJoint_setPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("IJoint_setPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   arg3 = reinterpret_cast< double * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   arg4 = reinterpret_cast< double * >(argp4);
   result = (bool)(arg1)->setPosLimits(arg2,*arg3,*arg4);
@@ -46407,26 +45657,23 @@ int _wrap_FixedJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_delete_MovableJointImpl1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
+int _wrap_IJoint_isRevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_MovableJointImpl1",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_isRevoluteJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl1" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_isRevoluteJoint" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (bool)iDynTree_IJoint_isRevoluteJoint((iDynTree::IJoint const *)arg1);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46434,23 +45681,23 @@ int _wrap_delete_MovableJointImpl1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_MovableJointImpl1_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
+int _wrap_IJoint_isFixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  bool result;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_isFixedJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_isFixedJoint" "', argument " "1"" of type '" "iDynTree::IJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (bool)iDynTree_IJoint_isFixedJoint((iDynTree::IJoint const *)arg1);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46458,23 +45705,23 @@ int _wrap_MovableJointImpl1_getNrOfPosCoords(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_MovableJointImpl1_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
+int _wrap_IJoint_asRevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IJoint_asRevoluteJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_asRevoluteJoint" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (iDynTree::RevoluteJoint *)iDynTree_IJoint_asRevoluteJoint(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46482,33 +45729,23 @@ int _wrap_MovableJointImpl1_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_MovableJointImpl1_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
-  iDynTree::JointIndex *arg2 = 0 ;
+int _wrap_IJoint_asFixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJoint *arg1 = (iDynTree::IJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  iDynTree::FixedJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IJoint_asFixedJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl1_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl1_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJoint_asFixedJoint" "', argument " "1"" of type '" "iDynTree::IJoint *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
-  (arg1)->setIndex(*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::IJoint * >(argp1);
+  result = (iDynTree::FixedJoint *)iDynTree_IJoint_asFixedJoint(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46516,23 +45753,42 @@ int _wrap_MovableJointImpl1_setIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl1_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_FixedJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkIndex arg1 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::Transform *arg3 = 0 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
+  iDynTree::FixedJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FixedJoint",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg1 = static_cast< iDynTree::LinkIndex >(val1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_FixedJoint" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_FixedJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getIndex();
-  _out = SWIG_From_int(static_cast< int >(result));
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FixedJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  result = (iDynTree::FixedJoint *)new iDynTree::FixedJoint(arg1,arg2,(iDynTree::Transform const &)*arg3);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46540,30 +45796,26 @@ int _wrap_MovableJointImpl1_getIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl1_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
-  size_t arg2 ;
-  void *argp1 = 0 ;
+int _wrap_new_FixedJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Transform *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::FixedJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_FixedJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Transform,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl1_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setPosCoordsOffset(arg2);
-  _out = (mxArray*)0;
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Transform * >(argp1);
+  result = (iDynTree::FixedJoint *)new iDynTree::FixedJoint((iDynTree::Transform const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46571,23 +45823,26 @@ int _wrap_MovableJointImpl1_setPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl1_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_FixedJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::FixedJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FixedJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__FixedJoint,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::FixedJoint const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getPosCoordsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FixedJoint" "', argument " "1"" of type '" "iDynTree::FixedJoint const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (iDynTree::FixedJoint *)new iDynTree::FixedJoint((iDynTree::FixedJoint const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FixedJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46595,29 +45850,75 @@ int _wrap_MovableJointImpl1_getPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl1_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
-  size_t arg2 ;
+int _wrap_new_FixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_FixedJoint__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FixedJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_FixedJoint__SWIG_2(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    {
+      int res = SWIG_AsVal_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_new_FixedJoint__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FixedJoint'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::FixedJoint::FixedJoint(iDynTree::LinkIndex const,iDynTree::LinkIndex const,iDynTree::Transform const &)\n"
+    "    iDynTree::FixedJoint::FixedJoint(iDynTree::Transform const &)\n"
+    "    iDynTree::FixedJoint::FixedJoint(iDynTree::FixedJoint const &)\n");
+  return 1;
+}
+
+
+int _wrap_delete_FixedJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_setDOFsOffset",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_FixedJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FixedJoint" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl1_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setDOFsOffset(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46626,23 +45927,23 @@ int _wrap_MovableJointImpl1_setDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_MovableJointImpl1_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
+int _wrap_FixedJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl1_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_clone" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getDOFsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (iDynTree::IJoint *)((iDynTree::FixedJoint const *)arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46650,26 +45951,23 @@ int _wrap_MovableJointImpl1_getDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_delete_MovableJointImpl2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
+int _wrap_FixedJoint_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  unsigned int result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_MovableJointImpl2",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl2" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (unsigned int)((iDynTree::FixedJoint const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46677,22 +45975,22 @@ int _wrap_delete_MovableJointImpl2(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_MovableJointImpl2_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
+int _wrap_FixedJoint_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getNrOfPosCoords();
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (unsigned int)((iDynTree::FixedJoint const *)arg1)->getNrOfDOFs();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46701,23 +45999,38 @@ int _wrap_MovableJointImpl2_getNrOfPosCoords(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_MovableJointImpl2_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
+int _wrap_FixedJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_setAttachedLinks",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  (arg1)->setAttachedLinks(arg2,arg3);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46725,32 +46038,32 @@ int _wrap_MovableJointImpl2_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_MovableJointImpl2_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
-  iDynTree::JointIndex *arg2 = 0 ;
+int _wrap_FixedJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::Transform *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_setRestTransform",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl2_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl2_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
-  (arg1)->setIndex(*arg2);
+  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
+  (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46759,22 +46072,22 @@ int _wrap_MovableJointImpl2_setIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl2_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
+int _wrap_FixedJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getFirstAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getIndex();
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::FixedJoint const *)arg1)->getFirstAttachedLink();
   _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -46783,30 +46096,23 @@ int _wrap_MovableJointImpl2_getIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl2_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
-  size_t arg2 ;
+int _wrap_FixedJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getSecondAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl2_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setPosCoordsOffset(arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::FixedJoint const *)arg1)->getSecondAttachedLink();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46814,23 +46120,39 @@ int _wrap_MovableJointImpl2_setPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl2_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
+int _wrap_FixedJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::Transform result;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getRestTransform",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getPosCoordsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = ((iDynTree::FixedJoint const *)arg1)->getRestTransform(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46838,30 +46160,50 @@ int _wrap_MovableJointImpl2_getPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl2_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
-  size_t arg2 ;
+int _wrap_FixedJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_setDOFsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getTransform",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getTransform" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl2_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setDOFsOffset(arg2);
-  _out = (mxArray*)0;
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  result = (iDynTree::Transform *) &((iDynTree::FixedJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46869,23 +46211,58 @@ int _wrap_MovableJointImpl2_setDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_MovableJointImpl2_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
+int _wrap_FixedJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
+  int arg5 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  int val5 ;
+  int ecode5 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::TransformDerivative result;
   
-  if (!SWIG_check_num_args("MovableJointImpl2_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getTransformDerivative",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getDOFsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "FixedJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
+  } 
+  arg5 = static_cast< int >(val5);
+  result = ((iDynTree::FixedJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
+  _out = SWIG_NewPointerObj((new iDynTree::TransformDerivative(static_cast< const iDynTree::TransformDerivative& >(result))), SWIGTYPE_p_iDynTree__TransformDerivative, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46893,26 +46270,47 @@ int _wrap_MovableJointImpl2_getDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_delete_MovableJointImpl3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+int _wrap_FixedJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  int arg2 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
+  iDynTree::SpatialMotionVector result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_MovableJointImpl3",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getMotionSubspaceVector",argc,4,4,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl3" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
+  } 
+  arg2 = static_cast< int >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  result = ((iDynTree::FixedJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
+  _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46920,23 +46318,104 @@ int _wrap_delete_MovableJointImpl3(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_MovableJointImpl3_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+int _wrap_FixedJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  iDynTree::LinkPositions *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  iDynTree::LinkAccArray *arg7 = 0 ;
+  iDynTree::LinkIndex arg8 ;
+  iDynTree::LinkIndex arg9 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
+  int val8 ;
+  int ecode8 = 0 ;
+  int val9 ;
+  int ecode9 = 0 ;
   mxArray * _out;
-  unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_computeChildPosVelAcc",argc,9,9,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
+  ecode8 = SWIG_AsVal_int(argv[7], &val8);
+  if (!SWIG_IsOK(ecode8)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg8 = static_cast< iDynTree::LinkIndex >(val8);
+  ecode9 = SWIG_AsVal_int(argv[8], &val9);
+  if (!SWIG_IsOK(ecode9)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "FixedJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg9 = static_cast< iDynTree::LinkIndex >(val9);
+  ((iDynTree::FixedJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46944,23 +46423,93 @@ int _wrap_MovableJointImpl3_getNrOfPosCoords(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_MovableJointImpl3_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+int _wrap_FixedJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  iDynTree::LinkVelArray *arg5 = 0 ;
+  iDynTree::LinkAccArray *arg6 = 0 ;
+  iDynTree::LinkIndex arg7 ;
+  iDynTree::LinkIndex arg8 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  int val7 ;
+  int ecode7 = 0 ;
+  int val8 ;
+  int ecode8 = 0 ;
   mxArray * _out;
-  unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_computeChildVelAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
+  ecode7 = SWIG_AsVal_int(argv[6], &val7);
+  if (!SWIG_IsOK(ecode7)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg7 = static_cast< iDynTree::LinkIndex >(val7);
+  ecode8 = SWIG_AsVal_int(argv[7], &val8);
+  if (!SWIG_IsOK(ecode8)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "FixedJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg8 = static_cast< iDynTree::LinkIndex >(val8);
+  ((iDynTree::FixedJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -46968,32 +46517,70 @@ int _wrap_MovableJointImpl3_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_MovableJointImpl3_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
-  iDynTree::JointIndex *arg2 = 0 ;
+int _wrap_FixedJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkIndex arg5 ;
+  iDynTree::LinkIndex arg6 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  int val5 ;
+  int ecode5 = 0 ;
+  int val6 ;
+  int ecode6 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_computeChildVel",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl3_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl3_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
-  (arg1)->setIndex(*arg2);
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "FixedJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg5 = static_cast< iDynTree::LinkIndex >(val5);
+  ecode6 = SWIG_AsVal_int(argv[5], &val6);
+  if (!SWIG_IsOK(ecode6)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "FixedJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg6 = static_cast< iDynTree::LinkIndex >(val6);
+  ((iDynTree::FixedJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47002,23 +46589,93 @@ int _wrap_MovableJointImpl3_setIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl3_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+int _wrap_FixedJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::VectorDynSize *arg5 = 0 ;
+  iDynTree::LinkAccArray *arg6 = 0 ;
+  iDynTree::LinkIndex arg7 ;
+  iDynTree::LinkIndex arg8 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  int val7 ;
+  int ecode7 = 0 ;
+  int val8 ;
+  int ecode8 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_computeChildAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getIndex();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
+  ecode7 = SWIG_AsVal_int(argv[6], &val7);
+  if (!SWIG_IsOK(ecode7)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "FixedJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg7 = static_cast< iDynTree::LinkIndex >(val7);
+  ecode8 = SWIG_AsVal_int(argv[7], &val8);
+  if (!SWIG_IsOK(ecode8)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "FixedJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg8 = static_cast< iDynTree::LinkIndex >(val8);
+  ((iDynTree::FixedJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47026,29 +46683,81 @@ int _wrap_MovableJointImpl3_getIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl3_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
-  size_t arg2 ;
+int _wrap_FixedJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkAccArray *arg5 = 0 ;
+  iDynTree::LinkIndex arg6 ;
+  iDynTree::LinkIndex arg7 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
+  int val6 ;
+  int ecode6 = 0 ;
+  int val7 ;
+  int ecode7 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_computeChildBiasAcc",argc,7,7,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl3_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
+  ecode6 = SWIG_AsVal_int(argv[5], &val6);
+  if (!SWIG_IsOK(ecode6)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setPosCoordsOffset(arg2);
+  arg6 = static_cast< iDynTree::LinkIndex >(val6);
+  ecode7 = SWIG_AsVal_int(argv[6], &val7);
+  if (!SWIG_IsOK(ecode7)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "FixedJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg7 = static_cast< iDynTree::LinkIndex >(val7);
+  ((iDynTree::FixedJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47057,23 +46766,71 @@ int _wrap_MovableJointImpl3_setPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl3_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+int _wrap_FixedJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::Wrench *arg3 = 0 ;
+  iDynTree::LinkIndex arg4 ;
+  iDynTree::LinkIndex arg5 ;
+  iDynTree::VectorDynSize *arg6 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  int val5 ;
+  int ecode5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
   mxArray * _out;
-  size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_computeJointTorque",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getPosCoordsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Wrench,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Wrench * >(argp3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "FixedJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "FixedJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg5 = static_cast< iDynTree::LinkIndex >(val5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "FixedJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
+  ((iDynTree::FixedJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47081,29 +46838,32 @@ int _wrap_MovableJointImpl3_getPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl3_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
-  size_t arg2 ;
+int _wrap_FixedJoint_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_setDOFsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setIndex" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl3_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->setDOFsOffset(arg2);
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FixedJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
+  (arg1)->setIndex(*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47112,23 +46872,23 @@ int _wrap_MovableJointImpl3_setDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_MovableJointImpl3_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+int _wrap_FixedJoint_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("MovableJointImpl3_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getIndex" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getDOFsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::FixedJoint const *)arg1)->getIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47136,25 +46896,29 @@ int _wrap_MovableJointImpl3_getDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_delete_MovableJointImpl4(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_MovableJointImpl4",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl4" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setPosCoordsOffset(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47163,23 +46927,23 @@ int _wrap_delete_MovableJointImpl4(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_MovableJointImpl4_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = ((iDynTree::FixedJoint const *)arg1)->getPosCoordsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47187,23 +46951,30 @@ int _wrap_MovableJointImpl4_getNrOfPosCoords(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_MovableJointImpl4_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setDOFsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47211,33 +46982,47 @@ int _wrap_MovableJointImpl4_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_MovableJointImpl4_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
-  iDynTree::JointIndex *arg2 = 0 ;
+int _wrap_FixedJoint_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl4_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = ((iDynTree::FixedJoint const *)arg1)->getDOFsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_FixedJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("FixedJoint_hasPosLimits",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl4_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
-  (arg1)->setIndex(*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  result = (bool)((iDynTree::FixedJoint const *)arg1)->hasPosLimits();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47245,23 +47030,31 @@ int _wrap_MovableJointImpl4_setIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl4_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
+  bool result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_enablePosLimits",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getIndex();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  result = (bool)(arg1)->enablePosLimits(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47269,30 +47062,53 @@ int _wrap_MovableJointImpl4_getIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl4_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   size_t arg2 ;
+  double *arg3 = 0 ;
+  double *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   size_t val2 ;
   int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl4_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  (arg1)->setPosCoordsOffset(arg2);
-  _out = (mxArray*)0;
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  arg3 = reinterpret_cast< double * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  arg4 = reinterpret_cast< double * >(argp4);
+  result = (bool)((iDynTree::FixedJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47300,23 +47116,31 @@ int _wrap_MovableJointImpl4_setPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl4_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  size_t result;
+  double result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getMinPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getPosCoordsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (double)((iDynTree::FixedJoint const *)arg1)->getMinPosLimit(arg2);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47324,30 +47148,31 @@ int _wrap_MovableJointImpl4_getPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl4_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   size_t val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
+  double result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_setDOFsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FixedJoint_getMaxPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::FixedJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl4_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  (arg1)->setDOFsOffset(arg2);
-  _out = (mxArray*)0;
+  result = (double)((iDynTree::FixedJoint const *)arg1)->getMaxPosLimit(arg2);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47355,23 +47180,53 @@ int _wrap_MovableJointImpl4_setDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_MovableJointImpl4_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+int _wrap_FixedJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FixedJoint *arg1 = (iDynTree::FixedJoint *) 0 ;
+  size_t arg2 ;
+  double *arg3 = 0 ;
+  double *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
-  size_t result;
+  bool result;
   
-  if (!SWIG_check_num_args("MovableJointImpl4_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FixedJoint_setPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FixedJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FixedJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::FixedJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getDOFsOffset();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FixedJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "FixedJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "FixedJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  arg3 = reinterpret_cast< double * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "FixedJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FixedJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  arg4 = reinterpret_cast< double * >(argp4);
+  result = (bool)(arg1)->setPosLimits(arg2,*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47379,22 +47234,22 @@ int _wrap_MovableJointImpl4_getDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_delete_MovableJointImpl5(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_delete_MovableJointImpl1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_MovableJointImpl5",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_MovableJointImpl1",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl5" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl1" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -47406,22 +47261,22 @@ int _wrap_delete_MovableJointImpl5(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_MovableJointImpl5_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getNrOfPosCoords();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getNrOfPosCoords();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47430,22 +47285,22 @@ int _wrap_MovableJointImpl5_getNrOfPosCoords(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_MovableJointImpl5_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getNrOfDOFs();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getNrOfDOFs();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47454,8 +47309,8 @@ int _wrap_MovableJointImpl5_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_MovableJointImpl5_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -47463,20 +47318,20 @@ int _wrap_MovableJointImpl5_setIndex(int resc, mxArray *resv[], int argc, mxArra
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl5_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl1_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl5_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl1_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
   (arg1)->setIndex(*arg2);
@@ -47488,22 +47343,22 @@ int _wrap_MovableJointImpl5_setIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl5_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getIndex();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getIndex();
   _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47512,8 +47367,8 @@ int _wrap_MovableJointImpl5_getIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl5_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -47521,17 +47376,17 @@ int _wrap_MovableJointImpl5_setPosCoordsOffset(int resc, mxArray *resv[], int ar
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl5_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl1_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   (arg1)->setPosCoordsOffset(arg2);
@@ -47543,22 +47398,22 @@ int _wrap_MovableJointImpl5_setPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl5_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getPosCoordsOffset();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getPosCoordsOffset();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47567,8 +47422,8 @@ int _wrap_MovableJointImpl5_getPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl5_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -47576,17 +47431,17 @@ int _wrap_MovableJointImpl5_setDOFsOffset(int resc, mxArray *resv[], int argc, m
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_setDOFsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl5_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl1_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   (arg1)->setDOFsOffset(arg2);
@@ -47598,22 +47453,22 @@ int _wrap_MovableJointImpl5_setDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_MovableJointImpl5_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+int _wrap_MovableJointImpl1_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 1,1 > *arg1 = (iDynTree::MovableJointImpl< 1,1 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl5_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl1_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl1_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 1,1 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getDOFsOffset();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 1,1 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 1,1 > const *)arg1)->getDOFsOffset();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47622,22 +47477,22 @@ int _wrap_MovableJointImpl5_getDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_delete_MovableJointImpl6(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_delete_MovableJointImpl2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_MovableJointImpl6",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_MovableJointImpl2",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl6" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl2" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -47649,22 +47504,22 @@ int _wrap_delete_MovableJointImpl6(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_MovableJointImpl6_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getNrOfPosCoords();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getNrOfPosCoords();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47673,22 +47528,22 @@ int _wrap_MovableJointImpl6_getNrOfPosCoords(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_MovableJointImpl6_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
-  result = (unsigned int)((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getNrOfDOFs();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getNrOfDOFs();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47697,8 +47552,8 @@ int _wrap_MovableJointImpl6_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_MovableJointImpl6_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -47706,20 +47561,20 @@ int _wrap_MovableJointImpl6_setIndex(int resc, mxArray *resv[], int argc, mxArra
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_setIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl6_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl2_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl6_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl2_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
   (arg1)->setIndex(*arg2);
@@ -47731,22 +47586,22 @@ int _wrap_MovableJointImpl6_setIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl6_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_getIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
-  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getIndex();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getIndex();
   _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47755,8 +47610,8 @@ int _wrap_MovableJointImpl6_getIndex(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_MovableJointImpl6_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -47764,17 +47619,17 @@ int _wrap_MovableJointImpl6_setPosCoordsOffset(int resc, mxArray *resv[], int ar
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_setPosCoordsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl6_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl2_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   (arg1)->setPosCoordsOffset(arg2);
@@ -47786,22 +47641,22 @@ int _wrap_MovableJointImpl6_setPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl6_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_getPosCoordsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getPosCoordsOffset();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getPosCoordsOffset();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47810,8 +47665,8 @@ int _wrap_MovableJointImpl6_getPosCoordsOffset(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_MovableJointImpl6_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -47819,17 +47674,17 @@ int _wrap_MovableJointImpl6_setDOFsOffset(int resc, mxArray *resv[], int argc, m
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_setDOFsOffset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl6_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl2_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   (arg1)->setDOFsOffset(arg2);
@@ -47841,22 +47696,22 @@ int _wrap_MovableJointImpl6_setDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_MovableJointImpl6_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+int _wrap_MovableJointImpl2_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 2,2 > *arg1 = (iDynTree::MovableJointImpl< 2,2 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("MovableJointImpl6_getDOFsOffset",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl2_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl2_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 2,2 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
-  result = ((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getDOFsOffset();
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 2,2 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 2,2 > const *)arg1)->getDOFsOffset();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -47865,16 +47720,26 @@ int _wrap_MovableJointImpl6_getDOFsOffset(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_new_RevoluteJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_delete_MovableJointImpl3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("new_RevoluteJoint",argc,0,0,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_MovableJointImpl3",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl3" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47882,53 +47747,23 @@ int _wrap_new_RevoluteJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_RevoluteJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkIndex arg1 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::Transform *arg3 = 0 ;
-  iDynTree::Axis *arg4 = 0 ;
-  int val1 ;
-  int ecode1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
+int _wrap_MovableJointImpl3_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::RevoluteJoint *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("new_RevoluteJoint",argc,4,4,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg1 = static_cast< iDynTree::LinkIndex >(val1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_RevoluteJoint" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_RevoluteJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Axis,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_RevoluteJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::Axis * >(argp4);
-  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint(arg1,arg2,(iDynTree::Transform const &)*arg3,(iDynTree::Axis const &)*arg4);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47936,37 +47771,23 @@ int _wrap_new_RevoluteJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_RevoluteJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Transform *arg1 = 0 ;
-  iDynTree::Axis *arg2 = 0 ;
-  void *argp1 ;
+int _wrap_MovableJointImpl3_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::RevoluteJoint *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("new_RevoluteJoint",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Transform,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Transform * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_RevoluteJoint" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
-  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint((iDynTree::Transform const &)*arg1,(iDynTree::Axis const &)*arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -47974,26 +47795,33 @@ int _wrap_new_RevoluteJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_RevoluteJoint__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_MovableJointImpl3_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  iDynTree::JointIndex *arg2 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("new_RevoluteJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__RevoluteJoint,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
   }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl3_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint((iDynTree::RevoluteJoint const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl3_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
+  (arg1)->setIndex(*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48001,89 +47829,53 @@ int _wrap_new_RevoluteJoint__SWIG_3(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_RevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_RevoluteJoint__SWIG_0(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_RevoluteJoint__SWIG_3(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_new_RevoluteJoint__SWIG_2(resc,resv,argc,argv);
-      }
-    }
+int _wrap_MovableJointImpl3_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::JointIndex result;
+  
+  if (!SWIG_check_num_args("MovableJointImpl3_getIndex",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 4) {
-    int _v;
-    {
-      int res = SWIG_AsVal_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_new_RevoluteJoint__SWIG_1(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_RevoluteJoint'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::RevoluteJoint::RevoluteJoint()\n"
-    "    iDynTree::RevoluteJoint::RevoluteJoint(iDynTree::LinkIndex const,iDynTree::LinkIndex const,iDynTree::Transform const &,iDynTree::Axis const &)\n"
-    "    iDynTree::RevoluteJoint::RevoluteJoint(iDynTree::Transform const &,iDynTree::Axis const &)\n"
-    "    iDynTree::RevoluteJoint::RevoluteJoint(iDynTree::RevoluteJoint const &)\n");
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_delete_RevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
+int _wrap_MovableJointImpl3_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_RevoluteJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
   }
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl3_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setPosCoordsOffset(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -48092,23 +47884,23 @@ int _wrap_delete_RevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_RevoluteJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
+int _wrap_MovableJointImpl3_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::IJoint *result = 0 ;
+  size_t result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_clone" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  result = (iDynTree::IJoint *)((iDynTree::RevoluteJoint const *)arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getPosCoordsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48116,37 +47908,29 @@ int _wrap_RevoluteJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_RevoluteJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_MovableJointImpl3_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  size_t val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("RevoluteJoint_setAttachedLinks",argc,3,3,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl3_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  (arg1)->setAttachedLinks(arg2,arg3);
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setDOFsOffset(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -48155,33 +47939,23 @@ int _wrap_RevoluteJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_RevoluteJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::Transform *arg2 = 0 ;
+int _wrap_MovableJointImpl3_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 3,3 > *arg1 = (iDynTree::MovableJointImpl< 3,3 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
+  size_t result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_setRestTransform",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl3_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl3_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 3,3 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
-  (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 3,3 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 3,3 > const *)arg1)->getDOFsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48189,48 +47963,25 @@ int _wrap_RevoluteJoint_setRestTransform(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_RevoluteJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::Axis *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
+int _wrap_delete_MovableJointImpl4(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("RevoluteJoint_setAxis",argc,4,4,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_MovableJointImpl4",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl4" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_setAxis" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  (arg1)->setAxis((iDynTree::Axis const &)*arg2,arg3,arg4);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -48239,41 +47990,47 @@ int _wrap_RevoluteJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::Axis *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_MovableJointImpl4_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_setAxis",argc,3,3,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl4_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_MovableJointImpl4_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  unsigned int result;
+  
+  if (!SWIG_check_num_args("MovableJointImpl4_getNrOfDOFs",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  (arg1)->setAxis((iDynTree::Axis const &)*arg2,arg3);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48281,32 +48038,32 @@ int _wrap_RevoluteJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::Axis *arg2 = 0 ;
+int _wrap_MovableJointImpl4_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+  iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("RevoluteJoint_setAxis",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl4_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl4_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl4_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
-  (arg1)->setAxis((iDynTree::Axis const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
+  (arg1)->setIndex(*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -48315,94 +48072,54 @@ int _wrap_RevoluteJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_RevoluteJoint_setAxis__SWIG_2(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_RevoluteJoint_setAxis__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
+int _wrap_MovableJointImpl4_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::JointIndex result;
+  
+  if (!SWIG_check_num_args("MovableJointImpl4_getIndex",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          {
-            int res = SWIG_AsVal_int(argv[3], NULL);
-            _v = SWIG_CheckState(res);
-          }
-          if (_v) {
-            return _wrap_RevoluteJoint_setAxis__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'RevoluteJoint_setAxis'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::RevoluteJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const,iDynTree::LinkIndex const)\n"
-    "    iDynTree::RevoluteJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const)\n"
-    "    iDynTree::RevoluteJoint::setAxis(iDynTree::Axis const &)\n");
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_RevoluteJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
+int _wrap_MovableJointImpl4_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getFirstAttachedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl4_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::RevoluteJoint const *)arg1)->getFirstAttachedLink();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl4_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setPosCoordsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48410,23 +48127,23 @@ int _wrap_RevoluteJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_RevoluteJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
+int _wrap_MovableJointImpl4_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  size_t result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getSecondAttachedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl4_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::RevoluteJoint const *)arg1)->getSecondAttachedLink();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getPosCoordsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48434,39 +48151,30 @@ int _wrap_RevoluteJoint_getSecondAttachedLink(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_RevoluteJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_MovableJointImpl4_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  size_t val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::Axis result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getAxis",argc,3,3,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl4_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl4_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::RevoluteJoint const *)arg1)->getAxis(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::Axis(static_cast< const iDynTree::Axis& >(result))), SWIGTYPE_p_iDynTree__Axis, SWIG_POINTER_OWN |  0 );
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setDOFsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48474,31 +48182,23 @@ int _wrap_RevoluteJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_MovableJointImpl4_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 4,4 > *arg1 = (iDynTree::MovableJointImpl< 4,4 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Axis result;
+  size_t result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getAxis",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl4_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl4_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 4,4 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = ((iDynTree::RevoluteJoint const *)arg1)->getAxis(arg2);
-  _out = SWIG_NewPointerObj((new iDynTree::Axis(static_cast< const iDynTree::Axis& >(result))), SWIGTYPE_p_iDynTree__Axis, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 4,4 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 4,4 > const *)arg1)->getDOFsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48506,85 +48206,74 @@ int _wrap_RevoluteJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_getAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_RevoluteJoint_getAxis__SWIG_1(resc,resv,argc,argv);
-      }
-    }
+int _wrap_delete_MovableJointImpl5(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_MovableJointImpl5",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_RevoluteJoint_getAxis__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl5" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_MovableJointImpl5_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  unsigned int result;
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'RevoluteJoint_getAxis'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::RevoluteJoint::getAxis(iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
-    "    iDynTree::RevoluteJoint::getAxis(iDynTree::LinkIndex const) const\n");
+  if (!SWIG_check_num_args("MovableJointImpl5_getNrOfPosCoords",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_RevoluteJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_MovableJointImpl5_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::Transform result;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getRestTransform",argc,3,3,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl5_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::RevoluteJoint const *)arg1)->getRestTransform(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48592,50 +48281,33 @@ int _wrap_RevoluteJoint_getRestTransform(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_RevoluteJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
+int _wrap_MovableJointImpl5_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+  iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getTransform",argc,4,4,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl5_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getTransform" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl5_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl5_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = (iDynTree::Transform *) &((iDynTree::RevoluteJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
+  (arg1)->setIndex(*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48643,58 +48315,23 @@ int _wrap_RevoluteJoint_getTransform(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_RevoluteJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
-  int arg5 ;
+int _wrap_MovableJointImpl5_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  int val5 ;
-  int ecode5 = 0 ;
   mxArray * _out;
-  iDynTree::TransformDerivative result;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getTransformDerivative",argc,5,5,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl5_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
-  } 
-  arg5 = static_cast< int >(val5);
-  result = ((iDynTree::RevoluteJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
-  _out = SWIG_NewPointerObj((new iDynTree::TransformDerivative(static_cast< const iDynTree::TransformDerivative& >(result))), SWIGTYPE_p_iDynTree__TransformDerivative, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48702,47 +48339,30 @@ int _wrap_RevoluteJoint_getTransformDerivative(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  int arg2 ;
-  iDynTree::LinkIndex arg3 ;
-  iDynTree::LinkIndex arg4 ;
+int _wrap_MovableJointImpl5_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  size_t val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getMotionSubspaceVector",argc,4,4,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl5_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
-  } 
-  arg2 = static_cast< int >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl5_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = ((iDynTree::RevoluteJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
-  _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setPosCoordsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48750,39 +48370,54 @@ int _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv[
 }
 
 
-int _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  int arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_MovableJointImpl5_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("MovableJointImpl5_getPosCoordsOffset",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getPosCoordsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_MovableJointImpl5_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+  size_t arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  size_t val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getMotionSubspaceVector",argc,3,3,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl5_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
-  } 
-  arg2 = static_cast< int >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl5_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::RevoluteJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->setDOFsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -48790,161 +48425,49 @@ int _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv[
 }
 
 
-int _wrap_RevoluteJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
+int _wrap_MovableJointImpl5_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 5,5 > *arg1 = (iDynTree::MovableJointImpl< 5,5 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("MovableJointImpl5_getDOFsOffset",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          {
-            int res = SWIG_AsVal_int(argv[3], NULL);
-            _v = SWIG_CheckState(res);
-          }
-          if (_v) {
-            return _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl5_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 5,5 > const *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'RevoluteJoint_getMotionSubspaceVector'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::RevoluteJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
-    "    iDynTree::RevoluteJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const) const\n");
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 5,5 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 5,5 > const *)arg1)->getDOFsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_RevoluteJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
-  iDynTree::LinkPositions *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  iDynTree::LinkAccArray *arg7 = 0 ;
-  iDynTree::LinkIndex arg8 ;
-  iDynTree::LinkIndex arg9 ;
+int _wrap_delete_MovableJointImpl6(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
-  int val8 ;
-  int ecode8 = 0 ;
-  int val9 ;
-  int ecode9 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("RevoluteJoint_computeChildPosVelAcc",argc,9,9,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_MovableJointImpl6",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MovableJointImpl6" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
   }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
-  ecode8 = SWIG_AsVal_int(argv[7], &val8);
-  if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ecode9 = SWIG_AsVal_int(argv[8], &val9);
-  if (!SWIG_IsOK(ecode9)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg9 = static_cast< iDynTree::LinkIndex >(val9);
-  ((iDynTree::RevoluteJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -48953,71 +48476,47 @@ int _wrap_RevoluteJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_RevoluteJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::LinkIndex arg5 ;
-  iDynTree::LinkIndex arg6 ;
+int _wrap_MovableJointImpl6_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  int val5 ;
-  int ecode5 = 0 ;
-  int val6 ;
-  int ecode6 = 0 ;
   mxArray * _out;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_computeChildVel",argc,6,6,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_MovableJointImpl6_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  unsigned int result;
+  
+  if (!SWIG_check_num_args("MovableJointImpl6_getNrOfDOFs",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "RevoluteJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg5 = static_cast< iDynTree::LinkIndex >(val5);
-  ecode6 = SWIG_AsVal_int(argv[5], &val6);
-  if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "RevoluteJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg6 = static_cast< iDynTree::LinkIndex >(val6);
-  ((iDynTree::RevoluteJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  result = (unsigned int)((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49025,92 +48524,32 @@ int _wrap_RevoluteJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
-  iDynTree::LinkVelArray *arg5 = 0 ;
-  iDynTree::LinkAccArray *arg6 = 0 ;
-  iDynTree::LinkIndex arg7 ;
-  iDynTree::LinkIndex arg8 ;
+int _wrap_MovableJointImpl6_setIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
+  iDynTree::JointIndex *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  int val7 ;
-  int ecode7 = 0 ;
-  int val8 ;
-  int ecode8 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("RevoluteJoint_computeChildVelAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_setIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_setIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_int,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MovableJointImpl6_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MovableJointImpl6_setIndex" "', argument " "2"" of type '" "iDynTree::JointIndex &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
-  ecode7 = SWIG_AsVal_int(argv[6], &val7);
-  if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ecode8 = SWIG_AsVal_int(argv[7], &val8);
-  if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::RevoluteJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
+  arg2 = reinterpret_cast< iDynTree::JointIndex * >(argp2);
+  (arg1)->setIndex(*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -49119,304 +48558,23 @@ int _wrap_RevoluteJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_RevoluteJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::VectorDynSize *arg5 = 0 ;
-  iDynTree::LinkAccArray *arg6 = 0 ;
-  iDynTree::LinkIndex arg7 ;
-  iDynTree::LinkIndex arg8 ;
+int _wrap_MovableJointImpl6_getIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  int val7 ;
-  int ecode7 = 0 ;
-  int val8 ;
-  int ecode8 = 0 ;
   mxArray * _out;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_computeChildAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_getIndex",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getIndex" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
-  ecode7 = SWIG_AsVal_int(argv[6], &val7);
-  if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ecode8 = SWIG_AsVal_int(argv[7], &val8);
-  if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::RevoluteJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_RevoluteJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::LinkAccArray *arg5 = 0 ;
-  iDynTree::LinkIndex arg6 ;
-  iDynTree::LinkIndex arg7 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  int val6 ;
-  int ecode6 = 0 ;
-  int val7 ;
-  int ecode7 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("RevoluteJoint_computeChildBiasAcc",argc,7,7,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
-  ecode6 = SWIG_AsVal_int(argv[5], &val6);
-  if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg6 = static_cast< iDynTree::LinkIndex >(val6);
-  ecode7 = SWIG_AsVal_int(argv[6], &val7);
-  if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ((iDynTree::RevoluteJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_RevoluteJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::Wrench *arg3 = 0 ;
-  iDynTree::LinkIndex arg4 ;
-  iDynTree::LinkIndex arg5 ;
-  iDynTree::VectorDynSize *arg6 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  int val5 ;
-  int ecode5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("RevoluteJoint_computeJointTorque",argc,6,6,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Wrench,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Wrench * >(argp3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg5 = static_cast< iDynTree::LinkIndex >(val5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
-  ((iDynTree::RevoluteJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_RevoluteJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("RevoluteJoint_hasPosLimits",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  result = (bool)((iDynTree::RevoluteJoint const *)arg1)->hasPosLimits();
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_RevoluteJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  bool arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  bool val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("RevoluteJoint_enablePosLimits",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
-  } 
-  arg2 = static_cast< bool >(val2);
-  result = (bool)(arg1)->enablePosLimits(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  result = (iDynTree::JointIndex)((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49424,53 +48582,30 @@ int _wrap_RevoluteJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_RevoluteJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
+int _wrap_MovableJointImpl6_setPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   size_t arg2 ;
-  double *arg3 = 0 ;
-  double *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   size_t val2 ;
   int ecode2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_setPosCoordsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_setPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl6_setPosCoordsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  arg3 = reinterpret_cast< double * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
-  }
-  arg4 = reinterpret_cast< double * >(argp4);
-  result = (bool)((iDynTree::RevoluteJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  (arg1)->setPosCoordsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49478,31 +48613,23 @@ int _wrap_RevoluteJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_RevoluteJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  size_t arg2 ;
+int _wrap_MovableJointImpl6_getPosCoordsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  double result;
+  size_t result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getMinPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_getPosCoordsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getPosCoordsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::RevoluteJoint const *)arg1)->getMinPosLimit(arg2);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getPosCoordsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49510,31 +48637,30 @@ int _wrap_RevoluteJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_RevoluteJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
+int _wrap_MovableJointImpl6_setDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   size_t val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  double result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_getMaxPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_setDOFsOffset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_setDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "MovableJointImpl6_setDOFsOffset" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::RevoluteJoint const *)arg1)->getMaxPosLimit(arg2);
-  _out = SWIG_From_double(static_cast< double >(result));
+  (arg1)->setDOFsOffset(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49542,53 +48668,23 @@ int _wrap_RevoluteJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_RevoluteJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
-  size_t arg2 ;
-  double *arg3 = 0 ;
-  double *arg4 = 0 ;
+int _wrap_MovableJointImpl6_getDOFsOffset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MovableJointImpl< 6,6 > *arg1 = (iDynTree::MovableJointImpl< 6,6 > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("RevoluteJoint_setPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("MovableJointImpl6_getDOFsOffset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
-  }
-  arg3 = reinterpret_cast< double * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MovableJointImpl6_getDOFsOffset" "', argument " "1"" of type '" "iDynTree::MovableJointImpl< 6,6 > const *""'"); 
   }
-  arg4 = reinterpret_cast< double * >(argp4);
-  result = (bool)(arg1)->setPosLimits(arg2,*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::MovableJointImpl< 6,6 > * >(argp1);
+  result = ((iDynTree::MovableJointImpl< 6,6 > const *)arg1)->getDOFsOffset();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49596,16 +48692,16 @@ int _wrap_RevoluteJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_new_PrismaticJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_RevoluteJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::PrismaticJoint *result = 0 ;
+  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("new_PrismaticJoint",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_RevoluteJoint",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::PrismaticJoint *)new iDynTree::PrismaticJoint();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__PrismaticJoint, 1 |  0 );
+  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49613,7 +48709,7 @@ int _wrap_new_PrismaticJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_new_PrismaticJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_RevoluteJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::LinkIndex arg1 ;
   iDynTree::LinkIndex arg2 ;
   iDynTree::Transform *arg3 = 0 ;
@@ -49627,39 +48723,39 @@ int _wrap_new_PrismaticJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArra
   void *argp4 ;
   int res4 = 0 ;
   mxArray * _out;
-  iDynTree::PrismaticJoint *result = 0 ;
+  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("new_PrismaticJoint",argc,4,4,0)) {
+  if (!SWIG_check_num_args("new_RevoluteJoint",argc,4,4,0)) {
     SWIG_fail;
   }
   ecode1 = SWIG_AsVal_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg1 = static_cast< iDynTree::LinkIndex >(val1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_PrismaticJoint" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_RevoluteJoint" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_PrismaticJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_RevoluteJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_PrismaticJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Axis,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_PrismaticJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_RevoluteJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_PrismaticJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::Axis * >(argp4);
-  result = (iDynTree::PrismaticJoint *)new iDynTree::PrismaticJoint(arg1,arg2,(iDynTree::Transform const &)*arg3,(iDynTree::Axis const &)*arg4);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__PrismaticJoint, 1 |  0 );
+  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint(arg1,arg2,(iDynTree::Transform const &)*arg3,(iDynTree::Axis const &)*arg4);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49667,26 +48763,37 @@ int _wrap_new_PrismaticJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_new_PrismaticJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = 0 ;
+int _wrap_new_RevoluteJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Transform *arg1 = 0 ;
+  iDynTree::Axis *arg2 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::PrismaticJoint *result = 0 ;
+  iDynTree::RevoluteJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("new_PrismaticJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_RevoluteJoint",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__PrismaticJoint,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Transform,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
-  result = (iDynTree::PrismaticJoint *)new iDynTree::PrismaticJoint((iDynTree::PrismaticJoint const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__PrismaticJoint, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Transform * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_RevoluteJoint" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
+  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint((iDynTree::Transform const &)*arg1,(iDynTree::Axis const &)*arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -49694,17 +48801,58 @@ int _wrap_new_PrismaticJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_new_PrismaticJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_RevoluteJoint__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::RevoluteJoint *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_RevoluteJoint",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__RevoluteJoint,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  result = (iDynTree::RevoluteJoint *)new iDynTree::RevoluteJoint((iDynTree::RevoluteJoint const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__RevoluteJoint, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_RevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_PrismaticJoint__SWIG_0(resc,resv,argc,argv);
+    return _wrap_new_RevoluteJoint__SWIG_0(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_PrismaticJoint__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_RevoluteJoint__SWIG_3(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_new_RevoluteJoint__SWIG_2(resc,resv,argc,argv);
+      }
     }
   }
   if (argc == 4) {
@@ -49727,38 +48875,39 @@ int _wrap_new_PrismaticJoint(int resc, mxArray *resv[], int argc, mxArray *argv[
           int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
           _v = SWIG_CheckState(res);
           if (_v) {
-            return _wrap_new_PrismaticJoint__SWIG_1(resc,resv,argc,argv);
+            return _wrap_new_RevoluteJoint__SWIG_1(resc,resv,argc,argv);
           }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_PrismaticJoint'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_RevoluteJoint'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::PrismaticJoint::PrismaticJoint()\n"
-    "    iDynTree::PrismaticJoint::PrismaticJoint(iDynTree::LinkIndex const,iDynTree::LinkIndex const,iDynTree::Transform const &,iDynTree::Axis const &)\n"
-    "    iDynTree::PrismaticJoint::PrismaticJoint(iDynTree::PrismaticJoint const &)\n");
-  return 1;
-}
-
+    "    iDynTree::RevoluteJoint::RevoluteJoint()\n"
+    "    iDynTree::RevoluteJoint::RevoluteJoint(iDynTree::LinkIndex const,iDynTree::LinkIndex const,iDynTree::Transform const &,iDynTree::Axis const &)\n"
+    "    iDynTree::RevoluteJoint::RevoluteJoint(iDynTree::Transform const &,iDynTree::Axis const &)\n"
+    "    iDynTree::RevoluteJoint::RevoluteJoint(iDynTree::RevoluteJoint const &)\n");
+  return 1;
+}
 
-int _wrap_delete_PrismaticJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+
+int _wrap_delete_RevoluteJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_PrismaticJoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_RevoluteJoint",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_RevoluteJoint" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -49770,22 +48919,22 @@ int _wrap_delete_PrismaticJoint(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_PrismaticJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("PrismaticJoint_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_clone" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_clone" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
-  result = (iDynTree::IJoint *)((iDynTree::PrismaticJoint const *)arg1)->clone();
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  result = (iDynTree::IJoint *)((iDynTree::RevoluteJoint const *)arg1)->clone();
   _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -49794,8 +48943,8 @@ int _wrap_PrismaticJoint_clone(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_PrismaticJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::LinkIndex arg2 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -49806,22 +48955,22 @@ int _wrap_PrismaticJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, m
   int ecode3 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_setAttachedLinks",argc,3,3,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_setAttachedLinks",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   (arg1)->setAttachedLinks(arg2,arg3);
@@ -49833,8 +48982,8 @@ int _wrap_PrismaticJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_PrismaticJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::Transform *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -49842,20 +48991,20 @@ int _wrap_PrismaticJoint_setRestTransform(int resc, mxArray *resv[], int argc, m
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_setRestTransform",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_setRestTransform",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
   (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
@@ -49867,8 +49016,8 @@ int _wrap_PrismaticJoint_setRestTransform(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_PrismaticJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::Axis *arg2 = 0 ;
   iDynTree::LinkIndex arg3 ;
   iDynTree::LinkIndex arg4 ;
@@ -49882,30 +49031,30 @@ int _wrap_PrismaticJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mx
   int ecode4 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_setAxis",argc,4,4,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_setAxis",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_setAxis" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_setAxis" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
   (arg1)->setAxis((iDynTree::Axis const &)*arg2,arg3,arg4);
@@ -49917,8 +49066,8 @@ int _wrap_PrismaticJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::Axis *arg2 = 0 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -49929,25 +49078,25 @@ int _wrap_PrismaticJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mx
   int ecode3 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_setAxis",argc,3,3,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_setAxis",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   (arg1)->setAxis((iDynTree::Axis const &)*arg2,arg3);
@@ -49959,8 +49108,8 @@ int _wrap_PrismaticJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::Axis *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -49968,20 +49117,20 @@ int _wrap_PrismaticJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mx
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_setAxis",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_setAxis",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
   (arg1)->setAxis((iDynTree::Axis const &)*arg2);
@@ -49993,25 +49142,25 @@ int _wrap_PrismaticJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_RevoluteJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
       int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_PrismaticJoint_setAxis__SWIG_2(resc,resv,argc,argv);
+        return _wrap_RevoluteJoint_setAxis__SWIG_2(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
@@ -50023,7 +49172,7 @@ int _wrap_PrismaticJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *a
           _v = SWIG_CheckState(res);
         }
         if (_v) {
-          return _wrap_PrismaticJoint_setAxis__SWIG_1(resc,resv,argc,argv);
+          return _wrap_RevoluteJoint_setAxis__SWIG_1(resc,resv,argc,argv);
         }
       }
     }
@@ -50031,7 +49180,7 @@ int _wrap_PrismaticJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *a
   if (argc == 4) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
@@ -50048,38 +49197,38 @@ int _wrap_PrismaticJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *a
             _v = SWIG_CheckState(res);
           }
           if (_v) {
-            return _wrap_PrismaticJoint_setAxis__SWIG_0(resc,resv,argc,argv);
+            return _wrap_RevoluteJoint_setAxis__SWIG_0(resc,resv,argc,argv);
           }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'PrismaticJoint_setAxis'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'RevoluteJoint_setAxis'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::PrismaticJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const,iDynTree::LinkIndex const)\n"
-    "    iDynTree::PrismaticJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const)\n"
-    "    iDynTree::PrismaticJoint::setAxis(iDynTree::Axis const &)\n");
+    "    iDynTree::RevoluteJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const,iDynTree::LinkIndex const)\n"
+    "    iDynTree::RevoluteJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const)\n"
+    "    iDynTree::RevoluteJoint::setAxis(iDynTree::Axis const &)\n");
   return 1;
 }
 
 
-int _wrap_PrismaticJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getFirstAttachedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getFirstAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::PrismaticJoint const *)arg1)->getFirstAttachedLink();
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::RevoluteJoint const *)arg1)->getFirstAttachedLink();
   _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50088,22 +49237,22 @@ int _wrap_PrismaticJoint_getFirstAttachedLink(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_PrismaticJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getSecondAttachedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getSecondAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::PrismaticJoint const *)arg1)->getSecondAttachedLink();
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::RevoluteJoint const *)arg1)->getSecondAttachedLink();
   _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50112,8 +49261,8 @@ int _wrap_PrismaticJoint_getSecondAttachedLink(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_PrismaticJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::LinkIndex arg2 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -50125,25 +49274,25 @@ int _wrap_PrismaticJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mx
   mxArray * _out;
   iDynTree::Axis result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getAxis",argc,3,3,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getAxis",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::PrismaticJoint const *)arg1)->getAxis(arg2,arg3);
+  result = ((iDynTree::RevoluteJoint const *)arg1)->getAxis(arg2,arg3);
   _out = SWIG_NewPointerObj((new iDynTree::Axis(static_cast< const iDynTree::Axis& >(result))), SWIGTYPE_p_iDynTree__Axis, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50152,8 +49301,8 @@ int _wrap_PrismaticJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -50162,20 +49311,20 @@ int _wrap_PrismaticJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mx
   mxArray * _out;
   iDynTree::Axis result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getAxis",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getAxis",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getAxis" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = ((iDynTree::PrismaticJoint const *)arg1)->getAxis(arg2);
+  result = ((iDynTree::RevoluteJoint const *)arg1)->getAxis(arg2);
   _out = SWIG_NewPointerObj((new iDynTree::Axis(static_cast< const iDynTree::Axis& >(result))), SWIGTYPE_p_iDynTree__Axis, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50184,11 +49333,11 @@ int _wrap_PrismaticJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_getAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_RevoluteJoint_getAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -50196,14 +49345,14 @@ int _wrap_PrismaticJoint_getAxis(int resc, mxArray *resv[], int argc, mxArray *a
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_PrismaticJoint_getAxis__SWIG_1(resc,resv,argc,argv);
+        return _wrap_RevoluteJoint_getAxis__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -50216,22 +49365,22 @@ int _wrap_PrismaticJoint_getAxis(int resc, mxArray *resv[], int argc, mxArray *a
           _v = SWIG_CheckState(res);
         }
         if (_v) {
-          return _wrap_PrismaticJoint_getAxis__SWIG_0(resc,resv,argc,argv);
+          return _wrap_RevoluteJoint_getAxis__SWIG_0(resc,resv,argc,argv);
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'PrismaticJoint_getAxis'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'RevoluteJoint_getAxis'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::PrismaticJoint::getAxis(iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
-    "    iDynTree::PrismaticJoint::getAxis(iDynTree::LinkIndex const) const\n");
+    "    iDynTree::RevoluteJoint::getAxis(iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
+    "    iDynTree::RevoluteJoint::getAxis(iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_PrismaticJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::LinkIndex arg2 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -50243,25 +49392,25 @@ int _wrap_PrismaticJoint_getRestTransform(int resc, mxArray *resv[], int argc, m
   mxArray * _out;
   iDynTree::Transform result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getRestTransform",argc,3,3,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getRestTransform",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::PrismaticJoint const *)arg1)->getRestTransform(arg2,arg3);
+  result = ((iDynTree::RevoluteJoint const *)arg1)->getRestTransform(arg2,arg3);
   _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50270,8 +49419,8 @@ int _wrap_PrismaticJoint_getRestTransform(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_PrismaticJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::LinkIndex arg3 ;
   iDynTree::LinkIndex arg4 ;
@@ -50286,33 +49435,33 @@ int _wrap_PrismaticJoint_getTransform(int resc, mxArray *resv[], int argc, mxArr
   mxArray * _out;
   iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getTransform",argc,4,4,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getTransform",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getTransform" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getTransform" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = (iDynTree::Transform *) &((iDynTree::PrismaticJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
+  result = (iDynTree::Transform *) &((iDynTree::RevoluteJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
   _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50321,8 +49470,8 @@ int _wrap_PrismaticJoint_getTransform(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_PrismaticJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::LinkIndex arg3 ;
   iDynTree::LinkIndex arg4 ;
@@ -50340,38 +49489,38 @@ int _wrap_PrismaticJoint_getTransformDerivative(int resc, mxArray *resv[], int a
   mxArray * _out;
   iDynTree::TransformDerivative result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getTransformDerivative",argc,5,5,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getTransformDerivative",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
   ecode5 = SWIG_AsVal_int(argv[4], &val5);
   if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "RevoluteJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
   } 
   arg5 = static_cast< int >(val5);
-  result = ((iDynTree::PrismaticJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
+  result = ((iDynTree::RevoluteJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
   _out = SWIG_NewPointerObj((new iDynTree::TransformDerivative(static_cast< const iDynTree::TransformDerivative& >(result))), SWIGTYPE_p_iDynTree__TransformDerivative, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50380,8 +49529,8 @@ int _wrap_PrismaticJoint_getTransformDerivative(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   int arg2 ;
   iDynTree::LinkIndex arg3 ;
   iDynTree::LinkIndex arg4 ;
@@ -50396,30 +49545,30 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv
   mxArray * _out;
   iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getMotionSubspaceVector",argc,4,4,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getMotionSubspaceVector",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
   } 
   arg2 = static_cast< int >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
-  result = ((iDynTree::PrismaticJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
+  result = ((iDynTree::RevoluteJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
   _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50428,8 +49577,8 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv
 }
 
 
-int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   int arg2 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -50441,25 +49590,25 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv
   mxArray * _out;
   iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getMotionSubspaceVector",argc,3,3,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getMotionSubspaceVector",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
   } 
   arg2 = static_cast< int >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "RevoluteJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::PrismaticJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3);
+  result = ((iDynTree::RevoluteJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3);
   _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50468,11 +49617,11 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv
 }
 
 
-int _wrap_PrismaticJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_RevoluteJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -50485,7 +49634,7 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int
           _v = SWIG_CheckState(res);
         }
         if (_v) {
-          return _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_1(resc,resv,argc,argv);
+          return _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_1(resc,resv,argc,argv);
         }
       }
     }
@@ -50493,7 +49642,7 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int
   if (argc == 4) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__RevoluteJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -50511,23 +49660,23 @@ int _wrap_PrismaticJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int
             _v = SWIG_CheckState(res);
           }
           if (_v) {
-            return _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_0(resc,resv,argc,argv);
+            return _wrap_RevoluteJoint_getMotionSubspaceVector__SWIG_0(resc,resv,argc,argv);
           }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'PrismaticJoint_getMotionSubspaceVector'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'RevoluteJoint_getMotionSubspaceVector'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::PrismaticJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
-    "    iDynTree::PrismaticJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const) const\n");
+    "    iDynTree::RevoluteJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
+    "    iDynTree::RevoluteJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_PrismaticJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::VectorDynSize *arg4 = 0 ;
@@ -50556,73 +49705,73 @@ int _wrap_PrismaticJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int ar
   int ecode9 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_computeChildPosVelAcc",argc,9,9,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_computeChildPosVelAcc",argc,9,9,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
   res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
   ecode8 = SWIG_AsVal_int(argv[7], &val8);
   if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg8 = static_cast< iDynTree::LinkIndex >(val8);
   ecode9 = SWIG_AsVal_int(argv[8], &val9);
   if (!SWIG_IsOK(ecode9)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "RevoluteJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg9 = static_cast< iDynTree::LinkIndex >(val9);
-  ((iDynTree::PrismaticJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
+  ((iDynTree::RevoluteJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50631,8 +49780,8 @@ int _wrap_PrismaticJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_PrismaticJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::LinkVelArray *arg4 = 0 ;
@@ -50652,49 +49801,49 @@ int _wrap_PrismaticJoint_computeChildVel(int resc, mxArray *resv[], int argc, mx
   int ecode6 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_computeChildVel",argc,6,6,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_computeChildVel",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
   ecode5 = SWIG_AsVal_int(argv[4], &val5);
   if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "PrismaticJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "RevoluteJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg5 = static_cast< iDynTree::LinkIndex >(val5);
   ecode6 = SWIG_AsVal_int(argv[5], &val6);
   if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "PrismaticJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "RevoluteJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg6 = static_cast< iDynTree::LinkIndex >(val6);
-  ((iDynTree::PrismaticJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
+  ((iDynTree::RevoluteJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50703,8 +49852,8 @@ int _wrap_PrismaticJoint_computeChildVel(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::VectorDynSize *arg4 = 0 ;
@@ -50730,65 +49879,65 @@ int _wrap_PrismaticJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc,
   int ecode8 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_computeChildVelAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_computeChildVelAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
   ecode7 = SWIG_AsVal_int(argv[6], &val7);
   if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg7 = static_cast< iDynTree::LinkIndex >(val7);
   ecode8 = SWIG_AsVal_int(argv[7], &val8);
   if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "RevoluteJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::PrismaticJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
+  ((iDynTree::RevoluteJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50797,8 +49946,8 @@ int _wrap_PrismaticJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_PrismaticJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::LinkVelArray *arg4 = 0 ;
@@ -50824,65 +49973,65 @@ int _wrap_PrismaticJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mx
   int ecode8 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_computeChildAcc",argc,8,8,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_computeChildAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
   ecode7 = SWIG_AsVal_int(argv[6], &val7);
   if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg7 = static_cast< iDynTree::LinkIndex >(val7);
   ecode8 = SWIG_AsVal_int(argv[7], &val8);
   if (!SWIG_IsOK(ecode8)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "RevoluteJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg8 = static_cast< iDynTree::LinkIndex >(val8);
-  ((iDynTree::PrismaticJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
+  ((iDynTree::RevoluteJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50891,8 +50040,8 @@ int _wrap_PrismaticJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::VectorDynSize *arg3 = 0 ;
   iDynTree::LinkVelArray *arg4 = 0 ;
@@ -50915,57 +50064,57 @@ int _wrap_PrismaticJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc
   int ecode7 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_computeChildBiasAcc",argc,7,7,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_computeChildBiasAcc",argc,7,7,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
   ecode6 = SWIG_AsVal_int(argv[5], &val6);
   if (!SWIG_IsOK(ecode6)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg6 = static_cast< iDynTree::LinkIndex >(val6);
   ecode7 = SWIG_AsVal_int(argv[6], &val7);
   if (!SWIG_IsOK(ecode7)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "RevoluteJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg7 = static_cast< iDynTree::LinkIndex >(val7);
-  ((iDynTree::PrismaticJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
+  ((iDynTree::RevoluteJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -50974,8 +50123,8 @@ int _wrap_PrismaticJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_PrismaticJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::Wrench *arg3 = 0 ;
   iDynTree::LinkIndex arg4 ;
@@ -50995,49 +50144,49 @@ int _wrap_PrismaticJoint_computeJointTorque(int resc, mxArray *resv[], int argc,
   int res6 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("PrismaticJoint_computeJointTorque",argc,6,6,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_computeJointTorque",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Wrench,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::Wrench * >(argp3);
   ecode4 = SWIG_AsVal_int(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg4 = static_cast< iDynTree::LinkIndex >(val4);
   ecode5 = SWIG_AsVal_int(argv[4], &val5);
   if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg5 = static_cast< iDynTree::LinkIndex >(val5);
   res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RevoluteJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
-  ((iDynTree::PrismaticJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
+  ((iDynTree::RevoluteJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -51046,22 +50195,22 @@ int _wrap_PrismaticJoint_computeJointTorque(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_PrismaticJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_hasPosLimits",argc,1,1,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_hasPosLimits",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
-  result = (bool)((iDynTree::PrismaticJoint const *)arg1)->hasPosLimits();
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
+  result = (bool)((iDynTree::RevoluteJoint const *)arg1)->hasPosLimits();
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -51070,8 +50219,8 @@ int _wrap_PrismaticJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_PrismaticJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -51080,17 +50229,17 @@ int _wrap_PrismaticJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mx
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_enablePosLimits",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_enablePosLimits",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_bool(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
   } 
   arg2 = static_cast< bool >(val2);
   result = (bool)(arg1)->enablePosLimits(arg2);
@@ -51102,8 +50251,8 @@ int _wrap_PrismaticJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_PrismaticJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   size_t arg2 ;
   double *arg3 = 0 ;
   double *arg4 = 0 ;
@@ -51118,36 +50267,36 @@ int _wrap_PrismaticJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArr
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   arg3 = reinterpret_cast< double * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   arg4 = reinterpret_cast< double * >(argp4);
-  result = (bool)((iDynTree::PrismaticJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
+  result = (bool)((iDynTree::RevoluteJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -51156,8 +50305,8 @@ int _wrap_PrismaticJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_PrismaticJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -51166,20 +50315,20 @@ int _wrap_PrismaticJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxA
   mxArray * _out;
   double result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getMinPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getMinPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::PrismaticJoint const *)arg1)->getMinPosLimit(arg2);
+  result = (double)((iDynTree::RevoluteJoint const *)arg1)->getMinPosLimit(arg2);
   _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -51188,8 +50337,8 @@ int _wrap_PrismaticJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_PrismaticJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -51198,20 +50347,20 @@ int _wrap_PrismaticJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxA
   mxArray * _out;
   double result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_getMaxPosLimit",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_getMaxPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::RevoluteJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
-  result = (double)((iDynTree::PrismaticJoint const *)arg1)->getMaxPosLimit(arg2);
+  result = (double)((iDynTree::RevoluteJoint const *)arg1)->getMaxPosLimit(arg2);
   _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -51220,8 +50369,8 @@ int _wrap_PrismaticJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_PrismaticJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+int _wrap_RevoluteJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::RevoluteJoint *arg1 = (iDynTree::RevoluteJoint *) 0 ;
   size_t arg2 ;
   double *arg3 = 0 ;
   double *arg4 = 0 ;
@@ -51236,33 +50385,33 @@ int _wrap_PrismaticJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArr
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("PrismaticJoint_setPosLimits",argc,4,4,0)) {
+  if (!SWIG_check_num_args("RevoluteJoint_setPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__RevoluteJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RevoluteJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::RevoluteJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::RevoluteJoint * >(argp1);
   ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "RevoluteJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
   } 
   arg2 = static_cast< size_t >(val2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RevoluteJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
   }
   arg3 = reinterpret_cast< double * >(argp3);
   res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RevoluteJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RevoluteJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
   }
   arg4 = reinterpret_cast< double * >(argp4);
   result = (bool)(arg1)->setPosLimits(arg2,*arg3,*arg4);
@@ -51274,16 +50423,16 @@ int _wrap_PrismaticJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_Traversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_PrismaticJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::Traversal *result = 0 ;
+  iDynTree::PrismaticJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("new_Traversal",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_PrismaticJoint",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::Traversal *)new iDynTree::Traversal();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Traversal, 1 |  0 );
+  result = (iDynTree::PrismaticJoint *)new iDynTree::PrismaticJoint();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__PrismaticJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51291,26 +50440,53 @@ int _wrap_new_Traversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_Traversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_PrismaticJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkIndex arg1 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::Transform *arg3 = 0 ;
+  iDynTree::Axis *arg4 = 0 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
+  iDynTree::PrismaticJoint *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_Traversal",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_PrismaticJoint",argc,4,4,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, SWIG_POINTER_DISOWN |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Traversal" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg1 = static_cast< iDynTree::LinkIndex >(val1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_PrismaticJoint" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_PrismaticJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_PrismaticJoint" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
-  _out = (mxArray*)0;
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Axis,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_PrismaticJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_PrismaticJoint" "', argument " "4"" of type '" "iDynTree::Axis const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Axis * >(argp4);
+  result = (iDynTree::PrismaticJoint *)new iDynTree::PrismaticJoint(arg1,arg2,(iDynTree::Transform const &)*arg3,(iDynTree::Axis const &)*arg4);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__PrismaticJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51318,23 +50494,26 @@ int _wrap_delete_Traversal(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_Traversal_getNrOfVisitedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_PrismaticJoint__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  iDynTree::PrismaticJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_getNrOfVisitedLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_PrismaticJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__PrismaticJoint,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getNrOfVisitedLinks" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  result = (unsigned int)((iDynTree::Traversal const *)arg1)->getNrOfVisitedLinks();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  result = (iDynTree::PrismaticJoint *)new iDynTree::PrismaticJoint((iDynTree::PrismaticJoint const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__PrismaticJoint, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51342,31 +50521,75 @@ int _wrap_Traversal_getNrOfVisitedLinks(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_Traversal_getLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::TraversalIndex arg2 ;
+int _wrap_new_PrismaticJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_PrismaticJoint__SWIG_0(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_PrismaticJoint__SWIG_2(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    {
+      int res = SWIG_AsVal_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_new_PrismaticJoint__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_PrismaticJoint'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::PrismaticJoint::PrismaticJoint()\n"
+    "    iDynTree::PrismaticJoint::PrismaticJoint(iDynTree::LinkIndex const,iDynTree::LinkIndex const,iDynTree::Transform const &,iDynTree::Axis const &)\n"
+    "    iDynTree::PrismaticJoint::PrismaticJoint(iDynTree::PrismaticJoint const &)\n");
+  return 1;
+}
+
+
+int _wrap_delete_PrismaticJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Link *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_getLink",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_PrismaticJoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getLink" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_PrismaticJoint" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getLink" "', argument " "2"" of type '" "iDynTree::TraversalIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::TraversalIndex >(val2);
-  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getLink(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51374,23 +50597,23 @@ int _wrap_Traversal_getLink(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_Traversal_getBaseLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+int _wrap_PrismaticJoint_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Link *result = 0 ;
+  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_getBaseLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getBaseLink" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_clone" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getBaseLink();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  result = (iDynTree::IJoint *)((iDynTree::PrismaticJoint const *)arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51398,31 +50621,38 @@ int _wrap_Traversal_getBaseLink(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_Traversal_getParentLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::TraversalIndex arg2 ;
+int _wrap_PrismaticJoint_setAttachedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::Link *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_getParentLink",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_setAttachedLinks",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentLink" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAttachedLinks" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentLink" "', argument " "2"" of type '" "iDynTree::TraversalIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_setAttachedLinks" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< iDynTree::TraversalIndex >(val2);
-  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getParentLink(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_setAttachedLinks" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  (arg1)->setAttachedLinks(arg2,arg3);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51430,63 +50660,33 @@ int _wrap_Traversal_getParentLink(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_Traversal_getParentJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::TraversalIndex arg2 ;
+int _wrap_PrismaticJoint_setRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::Transform *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_getParentJoint",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_setRestTransform",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentJoint" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setRestTransform" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentJoint" "', argument " "2"" of type '" "iDynTree::TraversalIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::TraversalIndex >(val2);
-  result = (iDynTree::IJoint *)((iDynTree::Traversal const *)arg1)->getParentJoint(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_Traversal_getParentLinkFromLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  iDynTree::Link *result = 0 ;
-  
-  if (!SWIG_check_num_args("Traversal_getParentLinkFromLinkIndex",argc,2,2,0)) {
-    SWIG_fail;
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentLinkFromLinkIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setRestTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentLinkFromLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getParentLinkFromLinkIndex(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
+  (arg1)->setRestTransform((iDynTree::Transform const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51494,63 +50694,49 @@ int _wrap_Traversal_getParentLinkFromLinkIndex(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_Traversal_getParentJointFromLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_PrismaticJoint_setAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::Axis *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
-  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_getParentJointFromLinkIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_setAxis",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentJointFromLinkIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentJointFromLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::IJoint *)((iDynTree::Traversal const *)arg1)->getParentJointFromLinkIndex(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_Traversal_getTraversalIndexFromLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  iDynTree::TraversalIndex result;
-  
-  if (!SWIG_check_num_args("Traversal_getTraversalIndexFromLinkIndex",argc,2,2,0)) {
-    SWIG_fail;
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getTraversalIndexFromLinkIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getTraversalIndexFromLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::TraversalIndex)((iDynTree::Traversal const *)arg1)->getTraversalIndexFromLinkIndex(arg2);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_setAxis" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  (arg1)->setAxis((iDynTree::Axis const &)*arg2,arg3,arg4);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51558,31 +50744,41 @@ int _wrap_Traversal_getTraversalIndexFromLinkIndex(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_Traversal_reset__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  unsigned int arg2 ;
+int _wrap_PrismaticJoint_setAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::Axis *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Traversal_reset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_setAxis",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_reset" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_reset" "', argument " "2"" of type '" "unsigned int""'");
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_setAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< unsigned int >(val2);
-  result = (bool)(arg1)->reset(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  (arg1)->setAxis((iDynTree::Axis const &)*arg2,arg3);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51590,34 +50786,33 @@ int _wrap_Traversal_reset__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_Traversal_reset__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_PrismaticJoint_setAxis__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::Axis *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Traversal_reset",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_setAxis",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_reset" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Axis,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_reset" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_reset" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setAxis" "', argument " "2"" of type '" "iDynTree::Axis const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)(arg1)->reset((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::Axis * >(argp2);
+  (arg1)->setAxis((iDynTree::Axis const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51625,70 +50820,94 @@ int _wrap_Traversal_reset__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_Traversal_reset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_PrismaticJoint_setAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_Traversal_reset__SWIG_1(resc,resv,argc,argv);
+        return _wrap_PrismaticJoint_setAxis__SWIG_2(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 2) {
+  if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_PrismaticJoint_setAxis__SWIG_1(resc,resv,argc,argv);
+        }
       }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Axis, 0);
+      _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_Traversal_reset__SWIG_0(resc,resv,argc,argv);
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          {
+            int res = SWIG_AsVal_int(argv[3], NULL);
+            _v = SWIG_CheckState(res);
+          }
+          if (_v) {
+            return _wrap_PrismaticJoint_setAxis__SWIG_0(resc,resv,argc,argv);
+          }
+        }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Traversal_reset'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'PrismaticJoint_setAxis'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Traversal::reset(unsigned int const)\n"
-    "    iDynTree::Traversal::reset(iDynTree::Model const &)\n");
+    "    iDynTree::PrismaticJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const,iDynTree::LinkIndex const)\n"
+    "    iDynTree::PrismaticJoint::setAxis(iDynTree::Axis const &,iDynTree::LinkIndex const)\n"
+    "    iDynTree::PrismaticJoint::setAxis(iDynTree::Axis const &)\n");
   return 1;
 }
 
 
-int _wrap_Traversal_addTraversalBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::Link *arg2 = (iDynTree::Link *) 0 ;
+int _wrap_PrismaticJoint_getFirstAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Traversal_addTraversalBase",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getFirstAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_addTraversalBase" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_addTraversalBase" "', argument " "2"" of type '" "iDynTree::Link const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getFirstAttachedLink" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Link * >(argp2);
-  result = (bool)(arg1)->addTraversalBase((iDynTree::Link const *)arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::PrismaticJoint const *)arg1)->getFirstAttachedLink();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51696,47 +50915,23 @@ int _wrap_Traversal_addTraversalBase(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_Traversal_addTraversalElement(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::Link *arg2 = (iDynTree::Link *) 0 ;
-  iDynTree::IJoint *arg3 = (iDynTree::IJoint *) 0 ;
-  iDynTree::Link *arg4 = (iDynTree::Link *) 0 ;
+int _wrap_PrismaticJoint_getSecondAttachedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Traversal_addTraversalElement",argc,4,4,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getSecondAttachedLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_addTraversalElement" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_addTraversalElement" "', argument " "2"" of type '" "iDynTree::Link const *""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Link * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Traversal_addTraversalElement" "', argument " "3"" of type '" "iDynTree::IJoint const *""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::IJoint * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Traversal_addTraversalElement" "', argument " "4"" of type '" "iDynTree::Link const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getSecondAttachedLink" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::Link * >(argp4);
-  result = (bool)(arg1)->addTraversalElement((iDynTree::Link const *)arg2,(iDynTree::IJoint const *)arg3,(iDynTree::Link const *)arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::PrismaticJoint const *)arg1)->getSecondAttachedLink();
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51744,8 +50939,8 @@ int _wrap_Traversal_addTraversalElement(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_Traversal_isParentOf(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+int _wrap_PrismaticJoint_getAxis__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
   iDynTree::LinkIndex arg2 ;
   iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
@@ -51755,28 +50950,28 @@ int _wrap_Traversal_isParentOf(int resc, mxArray *resv[], int argc, mxArray *arg
   int val3 ;
   int ecode3 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::Axis result;
   
-  if (!SWIG_check_num_args("Traversal_isParentOf",argc,3,3,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getAxis",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_isParentOf" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_isParentOf" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Traversal_isParentOf" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getAxis" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = (bool)((iDynTree::Traversal const *)arg1)->isParentOf(arg2,arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  result = ((iDynTree::PrismaticJoint const *)arg1)->getAxis(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::Axis(static_cast< const iDynTree::Axis& >(result))), SWIGTYPE_p_iDynTree__Axis, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51784,42 +50979,31 @@ int _wrap_Traversal_isParentOf(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_Traversal_getChildLinkIndexFromJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::JointIndex arg3 ;
+int _wrap_PrismaticJoint_getAxis__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  iDynTree::Axis result;
   
-  if (!SWIG_check_num_args("Traversal_getChildLinkIndexFromJointIndex",argc,3,3,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getAxis",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getAxis" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getAxis" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg3 = static_cast< iDynTree::JointIndex >(val3);
-  result = (iDynTree::LinkIndex)((iDynTree::Traversal const *)arg1)->getChildLinkIndexFromJointIndex((iDynTree::Model const &)*arg2,arg3);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = ((iDynTree::PrismaticJoint const *)arg1)->getAxis(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Axis(static_cast< const iDynTree::Axis& >(result))), SWIGTYPE_p_iDynTree__Axis, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51827,42 +51011,85 @@ int _wrap_Traversal_getChildLinkIndexFromJointIndex(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_Traversal_getParentLinkIndexFromJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::JointIndex arg3 ;
+int _wrap_PrismaticJoint_getAxis(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_PrismaticJoint_getAxis__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_PrismaticJoint_getAxis__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'PrismaticJoint_getAxis'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::PrismaticJoint::getAxis(iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
+    "    iDynTree::PrismaticJoint::getAxis(iDynTree::LinkIndex const) const\n");
+  return 1;
+}
+
+
+int _wrap_PrismaticJoint_getRestTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   int val3 ;
   int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  iDynTree::Transform result;
   
-  if (!SWIG_check_num_args("Traversal_getParentLinkIndexFromJointIndex",argc,3,3,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getRestTransform",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getRestTransform" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getRestTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
   ecode3 = SWIG_AsVal_int(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getRestTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg3 = static_cast< iDynTree::JointIndex >(val3);
-  result = (iDynTree::LinkIndex)((iDynTree::Traversal const *)arg1)->getParentLinkIndexFromJointIndex((iDynTree::Model const &)*arg2,arg3);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = ((iDynTree::PrismaticJoint const *)arg1)->getRestTransform(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51870,34 +51097,50 @@ int _wrap_Traversal_getParentLinkIndexFromJointIndex(int resc, mxArray *resv[],
 }
 
 
-int _wrap_Traversal_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
+int _wrap_PrismaticJoint_getTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
-  std::string result;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("Traversal_toString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getTransform",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_toString" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getTransform" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getTransform" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::Traversal const *)arg1)->toString((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getTransform" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_getTransform" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  result = (iDynTree::Transform *) &((iDynTree::PrismaticJoint const *)arg1)->getTransform((iDynTree::VectorDynSize const &)*arg2,arg3,arg4);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51905,26 +51148,58 @@ int _wrap_Traversal_toString(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_delete_SolidShape(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_getTransformDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
+  int arg5 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  int val5 ;
+  int ecode5 = 0 ;
   mxArray * _out;
+  iDynTree::TransformDerivative result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_SolidShape",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getTransformDerivative",argc,5,5,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_SolidShape" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "PrismaticJoint_getTransformDerivative" "', argument " "5"" of type '" "int""'");
+  } 
+  arg5 = static_cast< int >(val5);
+  result = ((iDynTree::PrismaticJoint const *)arg1)->getTransformDerivative((iDynTree::VectorDynSize const &)*arg2,arg3,arg4,arg5);
+  _out = SWIG_NewPointerObj((new iDynTree::TransformDerivative(static_cast< const iDynTree::TransformDerivative& >(result))), SWIGTYPE_p_iDynTree__TransformDerivative, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51932,23 +51207,47 @@ int _wrap_delete_SolidShape(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_SolidShape_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  int arg2 ;
+  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
-  iDynTree::SolidShape *result = 0 ;
+  iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("SolidShape_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getMotionSubspaceVector",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_clone" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::SolidShape *)(arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
+  } 
+  arg2 = static_cast< int >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  result = ((iDynTree::PrismaticJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3,arg4);
+  _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -51956,91 +51255,201 @@ int _wrap_SolidShape_clone(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_SolidShape_name_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  int arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  iDynTree::SpatialMotionVector result;
   
-  if (!SWIG_check_num_args("SolidShape_name_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getMotionSubspaceVector",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_name_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SolidShape_name_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SolidShape_name_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  if (arg1) (arg1)->name = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "2"" of type '" "int""'");
+  } 
+  arg2 = static_cast< int >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "PrismaticJoint_getMotionSubspaceVector" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = ((iDynTree::PrismaticJoint const *)arg1)->getMotionSubspaceVector(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::SpatialMotionVector(static_cast< const iDynTree::SpatialMotionVector& >(result))), SWIGTYPE_p_iDynTree__SpatialMotionVector, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_SolidShape_name_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  std::string *result = 0 ;
-  
-  if (!SWIG_check_num_args("SolidShape_name_get",argc,1,1,0)) {
-    SWIG_fail;
+int _wrap_PrismaticJoint_getMotionSubspaceVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_name_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__PrismaticJoint, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          {
+            int res = SWIG_AsVal_int(argv[3], NULL);
+            _v = SWIG_CheckState(res);
+          }
+          if (_v) {
+            return _wrap_PrismaticJoint_getMotionSubspaceVector__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (std::string *) & ((arg1)->name);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'PrismaticJoint_getMotionSubspaceVector'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::PrismaticJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const,iDynTree::LinkIndex const) const\n"
+    "    iDynTree::PrismaticJoint::getMotionSubspaceVector(int,iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_SolidShape_link_H_geometry_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
-  iDynTree::Transform *arg2 = (iDynTree::Transform *) 0 ;
+int _wrap_PrismaticJoint_computeChildPosVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  iDynTree::LinkPositions *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  iDynTree::LinkAccArray *arg7 = 0 ;
+  iDynTree::LinkIndex arg8 ;
+  iDynTree::LinkIndex arg9 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
+  int val8 ;
+  int ecode8 = 0 ;
+  int val9 ;
+  int ecode9 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("SolidShape_link_H_geometry_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_computeChildPosVelAcc",argc,9,9,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_link_H_geometry_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SolidShape_link_H_geometry_set" "', argument " "2"" of type '" "iDynTree::Transform *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
-  if (arg1) (arg1)->link_H_geometry = *arg2;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
+  ecode8 = SWIG_AsVal_int(argv[7], &val8);
+  if (!SWIG_IsOK(ecode8)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg8 = static_cast< iDynTree::LinkIndex >(val8);
+  ecode9 = SWIG_AsVal_int(argv[8], &val9);
+  if (!SWIG_IsOK(ecode9)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode9), "in method '" "PrismaticJoint_computeChildPosVelAcc" "', argument " "9"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg9 = static_cast< iDynTree::LinkIndex >(val9);
+  ((iDynTree::PrismaticJoint const *)arg1)->computeChildPosVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,*arg7,arg8,arg9);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -52049,23 +51458,71 @@ int _wrap_SolidShape_link_H_geometry_set(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_SolidShape_link_H_geometry_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_computeChildVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkIndex arg5 ;
+  iDynTree::LinkIndex arg6 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  int val5 ;
+  int ecode5 = 0 ;
+  int val6 ;
+  int ecode6 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("SolidShape_link_H_geometry_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_computeChildVel",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_link_H_geometry_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildVel" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Transform *)& ((arg1)->link_H_geometry);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVel" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVel" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVel" "', argument " "4"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "PrismaticJoint_computeChildVel" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg5 = static_cast< iDynTree::LinkIndex >(val5);
+  ecode6 = SWIG_AsVal_int(argv[5], &val6);
+  if (!SWIG_IsOK(ecode6)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "PrismaticJoint_computeChildVel" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg6 = static_cast< iDynTree::LinkIndex >(val6);
+  ((iDynTree::PrismaticJoint const *)arg1)->computeChildVel((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4,arg5,arg6);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52073,29 +51530,92 @@ int _wrap_SolidShape_link_H_geometry_get(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_SolidShape_material_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
-  iDynTree::Vector4 *arg2 = (iDynTree::Vector4 *) 0 ;
+int _wrap_PrismaticJoint_computeChildVelAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  iDynTree::LinkVelArray *arg5 = 0 ;
+  iDynTree::LinkAccArray *arg6 = 0 ;
+  iDynTree::LinkIndex arg7 ;
+  iDynTree::LinkIndex arg8 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  int val7 ;
+  int ecode7 = 0 ;
+  int val8 ;
+  int ecode8 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("SolidShape_material_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_computeChildVelAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_material_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SolidShape_material_set" "', argument " "2"" of type '" "iDynTree::Vector4 *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Vector4 * >(argp2);
-  if (arg1) (arg1)->material = *arg2;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "5"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
+  ecode7 = SWIG_AsVal_int(argv[6], &val7);
+  if (!SWIG_IsOK(ecode7)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg7 = static_cast< iDynTree::LinkIndex >(val7);
+  ecode8 = SWIG_AsVal_int(argv[7], &val8);
+  if (!SWIG_IsOK(ecode8)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "PrismaticJoint_computeChildVelAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg8 = static_cast< iDynTree::LinkIndex >(val8);
+  ((iDynTree::PrismaticJoint const *)arg1)->computeChildVelAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5,*arg6,arg7,arg8);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -52104,23 +51624,93 @@ int _wrap_SolidShape_material_set(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_SolidShape_material_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::Vector4 *result = 0 ;
+int _wrap_PrismaticJoint_computeChildAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::VectorDynSize *arg5 = 0 ;
+  iDynTree::LinkAccArray *arg6 = 0 ;
+  iDynTree::LinkIndex arg7 ;
+  iDynTree::LinkIndex arg8 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  int val7 ;
+  int ecode7 = 0 ;
+  int val8 ;
+  int ecode8 = 0 ;
+  mxArray * _out;
   
-  if (!SWIG_check_num_args("SolidShape_material_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_computeChildAcc",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_material_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Vector4 *)& ((arg1)->material);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "5"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildAcc" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
+  ecode7 = SWIG_AsVal_int(argv[6], &val7);
+  if (!SWIG_IsOK(ecode7)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg7 = static_cast< iDynTree::LinkIndex >(val7);
+  ecode8 = SWIG_AsVal_int(argv[7], &val8);
+  if (!SWIG_IsOK(ecode8)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode8), "in method '" "PrismaticJoint_computeChildAcc" "', argument " "8"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg8 = static_cast< iDynTree::LinkIndex >(val8);
+  ((iDynTree::PrismaticJoint const *)arg1)->computeChildAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::VectorDynSize const &)*arg5,*arg6,arg7,arg8);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52128,23 +51718,82 @@ int _wrap_SolidShape_material_get(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_SolidShape_isSphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_computeChildBiasAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkAccArray *arg5 = 0 ;
+  iDynTree::LinkIndex arg6 ;
+  iDynTree::LinkIndex arg7 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
+  int val6 ;
+  int ecode6 = 0 ;
+  int val7 ;
+  int ecode7 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("SolidShape_isSphere",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_computeChildBiasAcc",argc,7,7,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isSphere" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (bool)((iDynTree::SolidShape const *)arg1)->isSphere();
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "5"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
+  ecode6 = SWIG_AsVal_int(argv[5], &val6);
+  if (!SWIG_IsOK(ecode6)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "6"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg6 = static_cast< iDynTree::LinkIndex >(val6);
+  ecode7 = SWIG_AsVal_int(argv[6], &val7);
+  if (!SWIG_IsOK(ecode7)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode7), "in method '" "PrismaticJoint_computeChildBiasAcc" "', argument " "7"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg7 = static_cast< iDynTree::LinkIndex >(val7);
+  ((iDynTree::PrismaticJoint const *)arg1)->computeChildBiasAcc((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,*arg5,arg6,arg7);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52152,23 +51801,71 @@ int _wrap_SolidShape_isSphere(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_SolidShape_isBox(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_computeJointTorque(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::Wrench *arg3 = 0 ;
+  iDynTree::LinkIndex arg4 ;
+  iDynTree::LinkIndex arg5 ;
+  iDynTree::VectorDynSize *arg6 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  int val5 ;
+  int ecode5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("SolidShape_isBox",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_computeJointTorque",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isBox" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (bool)((iDynTree::SolidShape const *)arg1)->isBox();
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeJointTorque" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Wrench,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeJointTorque" "', argument " "3"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Wrench * >(argp3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "4"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::LinkIndex >(val4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "5"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg5 = static_cast< iDynTree::LinkIndex >(val5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "PrismaticJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_computeJointTorque" "', argument " "6"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
+  ((iDynTree::PrismaticJoint const *)arg1)->computeJointTorque((iDynTree::VectorDynSize const &)*arg2,(iDynTree::Wrench const &)*arg3,arg4,arg5,*arg6);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52176,22 +51873,22 @@ int _wrap_SolidShape_isBox(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_SolidShape_isCylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_hasPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SolidShape_isCylinder",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_hasPosLimits",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isCylinder" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_hasPosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (bool)((iDynTree::SolidShape const *)arg1)->isCylinder();
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  result = (bool)((iDynTree::PrismaticJoint const *)arg1)->hasPosLimits();
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -52200,22 +51897,30 @@ int _wrap_SolidShape_isCylinder(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_SolidShape_isExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_enablePosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SolidShape_isExternalMesh",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_enablePosLimits",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isExternalMesh" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_enablePosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (bool)((iDynTree::SolidShape const *)arg1)->isExternalMesh();
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_enablePosLimits" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  result = (bool)(arg1)->enablePosLimits(arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -52224,23 +51929,53 @@ int _wrap_SolidShape_isExternalMesh(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_SolidShape_asSphere__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_getPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  size_t arg2 ;
+  double *arg3 = 0 ;
+  double *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
-  iDynTree::Sphere *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("SolidShape_asSphere",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asSphere" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getPosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Sphere *)(arg1)->asSphere();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getPosLimits" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  arg3 = reinterpret_cast< double * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_getPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  arg4 = reinterpret_cast< double * >(argp4);
+  result = (bool)((iDynTree::PrismaticJoint const *)arg1)->getPosLimits(arg2,*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52248,23 +51983,31 @@ int _wrap_SolidShape_asSphere__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_SolidShape_asBox__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_getMinPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Box *result = 0 ;
+  double result;
   
-  if (!SWIG_check_num_args("SolidShape_asBox",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getMinPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asBox" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMinPosLimit" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Box *)(arg1)->asBox();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMinPosLimit" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (double)((iDynTree::PrismaticJoint const *)arg1)->getMinPosLimit(arg2);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52272,23 +52015,31 @@ int _wrap_SolidShape_asBox__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_SolidShape_asCylinder__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_getMaxPosLimit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Cylinder *result = 0 ;
+  double result;
   
-  if (!SWIG_check_num_args("SolidShape_asCylinder",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_getMaxPosLimit",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asCylinder" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_getMaxPosLimit" "', argument " "1"" of type '" "iDynTree::PrismaticJoint const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Cylinder *)(arg1)->asCylinder();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_getMaxPosLimit" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (double)((iDynTree::PrismaticJoint const *)arg1)->getMaxPosLimit(arg2);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52296,23 +52047,53 @@ int _wrap_SolidShape_asCylinder__SWIG_0(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_SolidShape_asExternalMesh__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_PrismaticJoint_setPosLimits(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::PrismaticJoint *arg1 = (iDynTree::PrismaticJoint *) 0 ;
+  size_t arg2 ;
+  double *arg3 = 0 ;
+  double *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
-  iDynTree::ExternalMesh *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("SolidShape_asExternalMesh",argc,1,1,0)) {
+  if (!SWIG_check_num_args("PrismaticJoint_setPosLimits",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__PrismaticJoint, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asExternalMesh" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "PrismaticJoint_setPosLimits" "', argument " "1"" of type '" "iDynTree::PrismaticJoint *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::ExternalMesh *)(arg1)->asExternalMesh();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::PrismaticJoint * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "PrismaticJoint_setPosLimits" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "PrismaticJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setPosLimits" "', argument " "3"" of type '" "double &""'"); 
+  }
+  arg3 = reinterpret_cast< double * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "PrismaticJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "PrismaticJoint_setPosLimits" "', argument " "4"" of type '" "double &""'"); 
+  }
+  arg4 = reinterpret_cast< double * >(argp4);
+  result = (bool)(arg1)->setPosLimits(arg2,*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52320,23 +52101,16 @@ int _wrap_SolidShape_asExternalMesh__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_SolidShape_asSphere__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_Traversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::Sphere *result = 0 ;
+  iDynTree::Traversal *result = 0 ;
   
-  if (!SWIG_check_num_args("SolidShape_asSphere",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_Traversal",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asSphere" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Sphere *)((iDynTree::SolidShape const *)arg1)->asSphere();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
+  (void)argv;
+  result = (iDynTree::Traversal *)new iDynTree::Traversal();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Traversal, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52344,51 +52118,50 @@ int _wrap_SolidShape_asSphere__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_SolidShape_asSphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asSphere__SWIG_0(resc,resv,argc,argv);
-    }
+int _wrap_delete_Traversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_Traversal",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asSphere__SWIG_1(resc,resv,argc,argv);
-    }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Traversal" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asSphere'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SolidShape::asSphere()\n"
-    "    iDynTree::SolidShape::asSphere() const\n");
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_SolidShape_asBox__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_Traversal_getNrOfVisitedLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Box *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("SolidShape_asBox",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getNrOfVisitedLinks",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asBox" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getNrOfVisitedLinks" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Box *)((iDynTree::SolidShape const *)arg1)->asBox();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  result = (unsigned int)((iDynTree::Traversal const *)arg1)->getNrOfVisitedLinks();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52396,51 +52169,31 @@ int _wrap_SolidShape_asBox__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_SolidShape_asBox(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asBox__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asBox__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asBox'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SolidShape::asBox()\n"
-    "    iDynTree::SolidShape::asBox() const\n");
-  return 1;
-}
-
-
-int _wrap_SolidShape_asCylinder__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_Traversal_getLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::TraversalIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Cylinder *result = 0 ;
+  iDynTree::Link *result = 0 ;
   
-  if (!SWIG_check_num_args("SolidShape_asCylinder",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getLink",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asCylinder" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getLink" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::Cylinder *)((iDynTree::SolidShape const *)arg1)->asCylinder();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getLink" "', argument " "2"" of type '" "iDynTree::TraversalIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::TraversalIndex >(val2);
+  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getLink(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52448,51 +52201,23 @@ int _wrap_SolidShape_asCylinder__SWIG_1(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_SolidShape_asCylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asCylinder__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asCylinder__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asCylinder'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SolidShape::asCylinder()\n"
-    "    iDynTree::SolidShape::asCylinder() const\n");
-  return 1;
-}
-
-
-int _wrap_SolidShape_asExternalMesh__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+int _wrap_Traversal_getBaseLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::ExternalMesh *result = 0 ;
+  iDynTree::Link *result = 0 ;
   
-  if (!SWIG_check_num_args("SolidShape_asExternalMesh",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getBaseLink",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asExternalMesh" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getBaseLink" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
-  result = (iDynTree::ExternalMesh *)((iDynTree::SolidShape const *)arg1)->asExternalMesh();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getBaseLink();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52500,54 +52225,31 @@ int _wrap_SolidShape_asExternalMesh__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_SolidShape_asExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asExternalMesh__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_SolidShape_asExternalMesh__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asExternalMesh'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SolidShape::asExternalMesh()\n"
-    "    iDynTree::SolidShape::asExternalMesh() const\n");
-  return 1;
-}
-
-
-int _wrap_delete_Sphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
+int _wrap_Traversal_getParentLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::TraversalIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::Link *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_Sphere",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getParentLink",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Sphere" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentLink" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentLink" "', argument " "2"" of type '" "iDynTree::TraversalIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::TraversalIndex >(val2);
+  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getParentLink(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52555,23 +52257,31 @@ int _wrap_delete_Sphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Sphere_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
+int _wrap_Traversal_getParentJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::TraversalIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SolidShape *result = 0 ;
+  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("Sphere_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getParentJoint",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Sphere_clone" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentJoint" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
-  result = (iDynTree::SolidShape *)(arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentJoint" "', argument " "2"" of type '" "iDynTree::TraversalIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::TraversalIndex >(val2);
+  result = (iDynTree::IJoint *)((iDynTree::Traversal const *)arg1)->getParentJoint(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52579,30 +52289,31 @@ int _wrap_Sphere_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Sphere_radius_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
-  double arg2 ;
+int _wrap_Traversal_getParentLinkFromLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
+  int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::Link *result = 0 ;
   
-  if (!SWIG_check_num_args("Sphere_radius_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Traversal_getParentLinkFromLinkIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Sphere_radius_set" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentLinkFromLinkIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Sphere_radius_set" "', argument " "2"" of type '" "double""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentLinkFromLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< double >(val2);
-  if (arg1) (arg1)->radius = arg2;
-  _out = (mxArray*)0;
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::Link *)((iDynTree::Traversal const *)arg1)->getParentLinkFromLinkIndex(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52610,23 +52321,31 @@ int _wrap_Sphere_radius_set(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_Sphere_radius_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
+int _wrap_Traversal_getParentJointFromLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  double result;
+  iDynTree::IJoint *result = 0 ;
   
-  if (!SWIG_check_num_args("Sphere_radius_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getParentJointFromLinkIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Sphere_radius_get" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentJointFromLinkIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
-  result = (double) ((arg1)->radius);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getParentJointFromLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::IJoint *)((iDynTree::Traversal const *)arg1)->getParentJointFromLinkIndex(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52634,16 +52353,31 @@ int _wrap_Sphere_radius_get(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_new_Sphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Traversal_getTraversalIndexFromLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Sphere *result = 0 ;
+  iDynTree::TraversalIndex result;
   
-  if (!SWIG_check_num_args("new_Sphere",argc,0,0,0)) {
+  if (!SWIG_check_num_args("Traversal_getTraversalIndexFromLinkIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::Sphere *)new iDynTree::Sphere();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sphere, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getTraversalIndexFromLinkIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_getTraversalIndexFromLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::TraversalIndex)((iDynTree::Traversal const *)arg1)->getTraversalIndexFromLinkIndex(arg2);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52651,26 +52385,31 @@ int _wrap_new_Sphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_Box(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+int _wrap_Traversal_reset__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  unsigned int arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_Box",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_reset",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Box" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_reset" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_reset" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  result = (bool)(arg1)->reset(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52678,23 +52417,34 @@ int _wrap_delete_Box(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+int _wrap_Traversal_reset__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::SolidShape *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("Box_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_reset",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_clone" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_reset" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  result = (iDynTree::SolidShape *)(arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_reset" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_reset" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)(arg1)->reset((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52702,30 +52452,70 @@ int _wrap_Box_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_x_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
-  double arg2 ;
+int _wrap_Traversal_reset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_Traversal_reset__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_Traversal_reset__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Traversal_reset'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::Traversal::reset(unsigned int const)\n"
+    "    iDynTree::Traversal::reset(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_Traversal_addTraversalBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::Link *arg2 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("Box_x_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Traversal_addTraversalBase",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_x_set" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_addTraversalBase" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Box_x_set" "', argument " "2"" of type '" "double""'");
-  } 
-  arg2 = static_cast< double >(val2);
-  if (arg1) (arg1)->x = arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_addTraversalBase" "', argument " "2"" of type '" "iDynTree::Link const *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Link * >(argp2);
+  result = (bool)(arg1)->addTraversalBase((iDynTree::Link const *)arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52733,23 +52523,47 @@ int _wrap_Box_x_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_x_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+int _wrap_Traversal_addTraversalElement(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::Link *arg2 = (iDynTree::Link *) 0 ;
+  iDynTree::IJoint *arg3 = (iDynTree::IJoint *) 0 ;
+  iDynTree::Link *arg4 = (iDynTree::Link *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
-  double result;
+  bool result;
   
-  if (!SWIG_check_num_args("Box_x_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_addTraversalElement",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_x_get" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_addTraversalElement" "', argument " "1"" of type '" "iDynTree::Traversal *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  result = (double) ((arg1)->x);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_addTraversalElement" "', argument " "2"" of type '" "iDynTree::Link const *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Link * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Traversal_addTraversalElement" "', argument " "3"" of type '" "iDynTree::IJoint const *""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::IJoint * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4,SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Traversal_addTraversalElement" "', argument " "4"" of type '" "iDynTree::Link const *""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Link * >(argp4);
+  result = (bool)(arg1)->addTraversalElement((iDynTree::Link const *)arg2,(iDynTree::IJoint const *)arg3,(iDynTree::Link const *)arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52757,30 +52571,39 @@ int _wrap_Box_x_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_y_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
-  double arg2 ;
+int _wrap_Traversal_isParentOf(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
+  int val2 ;
   int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("Box_y_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Traversal_isParentOf",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_y_set" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_isParentOf" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Box_y_set" "', argument " "2"" of type '" "double""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Traversal_isParentOf" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg2 = static_cast< double >(val2);
-  if (arg1) (arg1)->y = arg2;
-  _out = (mxArray*)0;
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Traversal_isParentOf" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = (bool)((iDynTree::Traversal const *)arg1)->isParentOf(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52788,23 +52611,42 @@ int _wrap_Box_y_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_y_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+int _wrap_Traversal_getChildLinkIndexFromJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::JointIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  double result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Box_y_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_getChildLinkIndexFromJointIndex",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_y_get" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  result = (double) ((arg1)->y);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Traversal_getChildLinkIndexFromJointIndex" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::JointIndex >(val3);
+  result = (iDynTree::LinkIndex)((iDynTree::Traversal const *)arg1)->getChildLinkIndexFromJointIndex((iDynTree::Model const &)*arg2,arg3);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52812,30 +52654,42 @@ int _wrap_Box_y_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_z_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
-  double arg2 ;
+int _wrap_Traversal_getParentLinkIndexFromJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::JointIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Box_z_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Traversal_getParentLinkIndexFromJointIndex",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_z_set" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Box_z_set" "', argument " "2"" of type '" "double""'");
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Traversal_getParentLinkIndexFromJointIndex" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
   } 
-  arg2 = static_cast< double >(val2);
-  if (arg1) (arg1)->z = arg2;
-  _out = (mxArray*)0;
+  arg3 = static_cast< iDynTree::JointIndex >(val3);
+  result = (iDynTree::LinkIndex)((iDynTree::Traversal const *)arg1)->getParentLinkIndexFromJointIndex((iDynTree::Model const &)*arg2,arg3);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52843,40 +52697,34 @@ int _wrap_Box_z_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Box_z_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+int _wrap_Traversal_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Traversal *arg1 = (iDynTree::Traversal *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  double result;
+  std::string result;
   
-  if (!SWIG_check_num_args("Box_z_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Traversal_toString",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_z_get" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Traversal_toString" "', argument " "1"" of type '" "iDynTree::Traversal const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
-  result = (double) ((arg1)->z);
-  _out = SWIG_From_double(static_cast< double >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_Box(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::Box *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_Box",argc,0,0,0)) {
-    SWIG_fail;
+  arg1 = reinterpret_cast< iDynTree::Traversal * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Traversal_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  (void)argv;
-  result = (iDynTree::Box *)new iDynTree::Box();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Box, 1 |  0 );
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Traversal_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = ((iDynTree::Traversal const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52884,22 +52732,22 @@ int _wrap_new_Box(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_Cylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
+int _wrap_delete_SolidShape(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_Cylinder",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_SolidShape",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Cylinder" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_SolidShape" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -52911,21 +52759,21 @@ int _wrap_delete_Cylinder(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_Cylinder_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
+int _wrap_SolidShape_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::SolidShape *result = 0 ;
   
-  if (!SWIG_check_num_args("Cylinder_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_clone" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_clone" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
   result = (iDynTree::SolidShape *)(arg1)->clone();
   _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
@@ -52935,54 +52783,61 @@ int _wrap_Cylinder_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Cylinder_length_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
-  double arg2 ;
+int _wrap_SolidShape_name_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
-  int ecode2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("Cylinder_length_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SolidShape_name_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_length_set" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_name_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Cylinder_length_set" "', argument " "2"" of type '" "double""'");
-  } 
-  arg2 = static_cast< double >(val2);
-  if (arg1) (arg1)->length = arg2;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SolidShape_name_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SolidShape_name_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  if (arg1) (arg1)->name = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Cylinder_length_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
+int _wrap_SolidShape_name_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  double result;
+  std::string *result = 0 ;
   
-  if (!SWIG_check_num_args("Cylinder_length_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_name_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_length_get" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_name_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
-  result = (double) ((arg1)->length);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (std::string *) & ((arg1)->name);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -52990,29 +52845,29 @@ int _wrap_Cylinder_length_get(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_Cylinder_radius_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
-  double arg2 ;
+int _wrap_SolidShape_link_H_geometry_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  iDynTree::Transform *arg2 = (iDynTree::Transform *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("Cylinder_radius_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SolidShape_link_H_geometry_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_radius_set" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_link_H_geometry_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Cylinder_radius_set" "', argument " "2"" of type '" "double""'");
-  } 
-  arg2 = static_cast< double >(val2);
-  if (arg1) (arg1)->radius = arg2;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SolidShape_link_H_geometry_set" "', argument " "2"" of type '" "iDynTree::Transform *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
+  if (arg1) (arg1)->link_H_geometry = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -53021,23 +52876,23 @@ int _wrap_Cylinder_radius_set(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_Cylinder_radius_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
+int _wrap_SolidShape_link_H_geometry_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  double result;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("Cylinder_radius_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_link_H_geometry_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_radius_get" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_link_H_geometry_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
-  result = (double) ((arg1)->radius);
-  _out = SWIG_From_double(static_cast< double >(result));
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Transform *)& ((arg1)->link_H_geometry);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53045,16 +52900,30 @@ int _wrap_Cylinder_radius_get(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_new_Cylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_SolidShape_material_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  iDynTree::Vector4 *arg2 = (iDynTree::Vector4 *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Cylinder *result = 0 ;
   
-  if (!SWIG_check_num_args("new_Cylinder",argc,0,0,0)) {
+  if (!SWIG_check_num_args("SolidShape_material_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::Cylinder *)new iDynTree::Cylinder();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Cylinder, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_material_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SolidShape_material_set" "', argument " "2"" of type '" "iDynTree::Vector4 *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Vector4 * >(argp2);
+  if (arg1) (arg1)->material = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53062,26 +52931,23 @@ int _wrap_new_Cylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_ExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
+int _wrap_SolidShape_material_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::Vector4 *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_ExternalMesh",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_material_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ExternalMesh" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_material_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Vector4 *)& ((arg1)->material);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53089,23 +52955,23 @@ int _wrap_delete_ExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_ExternalMesh_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
+int _wrap_SolidShape_isSphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SolidShape *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("ExternalMesh_clone",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_isSphere",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_clone" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isSphere" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
-  result = (iDynTree::SolidShape *)(arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (bool)((iDynTree::SolidShape const *)arg1)->isSphere();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53113,61 +52979,47 @@ int _wrap_ExternalMesh_clone(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_ExternalMesh_filename_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_SolidShape_isBox(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ExternalMesh_filename_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SolidShape_isBox",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_filename_set" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExternalMesh_filename_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExternalMesh_filename_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isBox" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  if (arg1) (arg1)->filename = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (bool)((iDynTree::SolidShape const *)arg1)->isBox();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_ExternalMesh_filename_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
+int _wrap_SolidShape_isCylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  std::string *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("ExternalMesh_filename_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_isCylinder",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_filename_get" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isCylinder" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
-  result = (std::string *) & ((arg1)->filename);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (bool)((iDynTree::SolidShape const *)arg1)->isCylinder();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53175,30 +53027,23 @@ int _wrap_ExternalMesh_filename_get(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_ExternalMesh_scale_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
-  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
+int _wrap_SolidShape_isExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ExternalMesh_scale_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SolidShape_isExternalMesh",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_scale_set" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExternalMesh_scale_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_isExternalMesh" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
-  if (arg1) (arg1)->scale = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (bool)((iDynTree::SolidShape const *)arg1)->isExternalMesh();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53206,23 +53051,23 @@ int _wrap_ExternalMesh_scale_set(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_ExternalMesh_scale_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
+int _wrap_SolidShape_asSphere__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Vector3 *result = 0 ;
+  iDynTree::Sphere *result = 0 ;
   
-  if (!SWIG_check_num_args("ExternalMesh_scale_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_asSphere",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_scale_get" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asSphere" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
-  result = (iDynTree::Vector3 *)& ((arg1)->scale);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Sphere *)(arg1)->asSphere();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53230,16 +53075,23 @@ int _wrap_ExternalMesh_scale_get(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_new_ExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_SolidShape_asBox__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::ExternalMesh *result = 0 ;
+  iDynTree::Box *result = 0 ;
   
-  if (!SWIG_check_num_args("new_ExternalMesh",argc,0,0,0)) {
+  if (!SWIG_check_num_args("SolidShape_asBox",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::ExternalMesh *)new iDynTree::ExternalMesh();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExternalMesh, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asBox" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Box *)(arg1)->asBox();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Box, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53247,16 +53099,23 @@ int _wrap_new_ExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_new_ModelSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_SolidShape_asCylinder__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::ModelSolidShapes *result = 0 ;
+  iDynTree::Cylinder *result = 0 ;
   
-  if (!SWIG_check_num_args("new_ModelSolidShapes",argc,0,0,0)) {
+  if (!SWIG_check_num_args("SolidShape_asCylinder",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::ModelSolidShapes *)new iDynTree::ModelSolidShapes();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asCylinder" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Cylinder *)(arg1)->asCylinder();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53264,26 +53123,47 @@ int _wrap_new_ModelSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_new_ModelSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_SolidShape_asExternalMesh__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::ModelSolidShapes *result = 0 ;
+  iDynTree::ExternalMesh *result = 0 ;
   
-  if (!SWIG_check_num_args("new_ModelSolidShapes",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_asExternalMesh",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__ModelSolidShapes,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_ModelSolidShapes" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asExternalMesh" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
   }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_ModelSolidShapes" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::ExternalMesh *)(arg1)->asExternalMesh();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_SolidShape_asSphere__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Sphere *result = 0 ;
+  
+  if (!SWIG_check_num_args("SolidShape_asSphere",argc,1,1,0)) {
+    SWIG_fail;
   }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  result = (iDynTree::ModelSolidShapes *)new iDynTree::ModelSolidShapes((iDynTree::ModelSolidShapes const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asSphere" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Sphere *)((iDynTree::SolidShape const *)arg1)->asSphere();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53291,44 +53171,51 @@ int _wrap_new_ModelSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_new_ModelSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_ModelSolidShapes__SWIG_0(resc,resv,argc,argv);
+int _wrap_SolidShape_asSphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_SolidShape_asSphere__SWIG_0(resc,resv,argc,argv);
+    }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelSolidShapes, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_ModelSolidShapes__SWIG_1(resc,resv,argc,argv);
+      return _wrap_SolidShape_asSphere__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_ModelSolidShapes'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asSphere'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelSolidShapes::ModelSolidShapes()\n"
-    "    iDynTree::ModelSolidShapes::ModelSolidShapes(iDynTree::ModelSolidShapes const &)\n");
+    "    iDynTree::SolidShape::asSphere()\n"
+    "    iDynTree::SolidShape::asSphere() const\n");
   return 1;
 }
 
 
-int _wrap_ModelSolidShapes_clear(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+int _wrap_SolidShape_asBox__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::Box *result = 0 ;
   
-  if (!SWIG_check_num_args("ModelSolidShapes_clear",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_asBox",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_clear" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asBox" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  (arg1)->clear();
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Box *)((iDynTree::SolidShape const *)arg1)->asBox();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Box, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53336,26 +53223,51 @@ int _wrap_ModelSolidShapes_clear(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_delete_ModelSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+int _wrap_SolidShape_asBox(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_SolidShape_asBox__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_SolidShape_asBox__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asBox'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::SolidShape::asBox()\n"
+    "    iDynTree::SolidShape::asBox() const\n");
+  return 1;
+}
+
+
+int _wrap_SolidShape_asCylinder__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::Cylinder *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_ModelSolidShapes",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SolidShape_asCylinder",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelSolidShapes" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asCylinder" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::Cylinder *)((iDynTree::SolidShape const *)arg1)->asCylinder();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53363,64 +53275,51 @@ int _wrap_delete_ModelSolidShapes(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_ModelSolidShapes_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
-  size_t arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("ModelSolidShapes_resize",argc,2,2,0)) {
-    SWIG_fail;
+int _wrap_SolidShape_asCylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_SolidShape_asCylinder__SWIG_0(resc,resv,argc,argv);
+    }
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_resize" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_SolidShape_asCylinder__SWIG_1(resc,resv,argc,argv);
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ModelSolidShapes_resize" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asCylinder'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::SolidShape::asCylinder()\n"
+    "    iDynTree::SolidShape::asCylinder() const\n");
   return 1;
 }
 
 
-int _wrap_ModelSolidShapes_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_SolidShape_asExternalMesh__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
+  iDynTree::ExternalMesh *result = 0 ;
   
-  if (!SWIG_check_num_args("ModelSolidShapes_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SolidShape_asExternalMesh",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_resize" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelSolidShapes_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelSolidShapes_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_asExternalMesh" "', argument " "1"" of type '" "iDynTree::SolidShape const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (iDynTree::ExternalMesh *)((iDynTree::SolidShape const *)arg1)->asExternalMesh();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53428,73 +53327,54 @@ int _wrap_ModelSolidShapes_resize__SWIG_1(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_ModelSolidShapes_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_SolidShape_asExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelSolidShapes, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_ModelSolidShapes_resize__SWIG_1(resc,resv,argc,argv);
-      }
+      return _wrap_SolidShape_asExternalMesh__SWIG_0(resc,resv,argc,argv);
     }
   }
-  if (argc == 2) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelSolidShapes, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SolidShape, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_ModelSolidShapes_resize__SWIG_0(resc,resv,argc,argv);
-      }
+      return _wrap_SolidShape_asExternalMesh__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelSolidShapes_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SolidShape_asExternalMesh'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelSolidShapes::resize(size_t)\n"
-    "    iDynTree::ModelSolidShapes::resize(iDynTree::Model const &)\n");
+    "    iDynTree::SolidShape::asExternalMesh()\n"
+    "    iDynTree::SolidShape::asExternalMesh() const\n");
   return 1;
 }
 
 
-int _wrap_ModelSolidShapes_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_delete_Sphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ModelSolidShapes_isConsistent",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_Sphere",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_isConsistent" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelSolidShapes_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Sphere" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelSolidShapes_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::ModelSolidShapes const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53502,30 +53382,23 @@ int _wrap_ModelSolidShapes_isConsistent(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_ModelSolidShapes_linkSolidShapes_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
-  std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *arg2 = (std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *) 0 ;
+int _wrap_Sphere_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  iDynTree::SolidShape *result = 0 ;
   
-  if (!SWIG_check_num_args("ModelSolidShapes_linkSolidShapes_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Sphere_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_linkSolidShapes_set" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelSolidShapes_linkSolidShapes_set" "', argument " "2"" of type '" "std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Sphere_clone" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > * >(argp2);
-  if (arg1) (arg1)->linkSolidShapes = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
+  result = (iDynTree::SolidShape *)(arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53533,23 +53406,30 @@ int _wrap_ModelSolidShapes_linkSolidShapes_set(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_ModelSolidShapes_linkSolidShapes_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+int _wrap_Sphere_radius_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *result = 0 ;
   
-  if (!SWIG_check_num_args("ModelSolidShapes_linkSolidShapes_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Sphere_radius_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_linkSolidShapes_get" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Sphere_radius_set" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
-  result = (std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *)& ((arg1)->linkSolidShapes);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Sphere_radius_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->radius = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53557,30 +53437,23 @@ int _wrap_ModelSolidShapes_linkSolidShapes_get(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_Neighbor_neighborLink_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_Sphere_radius_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Sphere *arg1 = (iDynTree::Sphere *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
+  double result;
   
-  if (!SWIG_check_num_args("Neighbor_neighborLink_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Sphere_radius_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Sphere, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborLink_set" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Sphere_radius_get" "', argument " "1"" of type '" "iDynTree::Sphere *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Neighbor_neighborLink_set" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  if (arg1) (arg1)->neighborLink = arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Sphere * >(argp1);
+  result = (double) ((arg1)->radius);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53588,23 +53461,16 @@ int _wrap_Neighbor_neighborLink_set(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_Neighbor_neighborLink_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_Sphere(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  iDynTree::Sphere *result = 0 ;
   
-  if (!SWIG_check_num_args("Neighbor_neighborLink_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_Sphere",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborLink_get" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
-  result = (iDynTree::LinkIndex) ((arg1)->neighborLink);
-  _out = SWIG_From_int(static_cast< int >(result));
+  (void)argv;
+  result = (iDynTree::Sphere *)new iDynTree::Sphere();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sphere, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53612,29 +53478,25 @@ int _wrap_Neighbor_neighborLink_get(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_Neighbor_neighborJoint_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
-  iDynTree::JointIndex arg2 ;
+int _wrap_delete_Box(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("Neighbor_neighborJoint_set",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_Box",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborJoint_set" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Box" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Neighbor_neighborJoint_set" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::JointIndex >(val2);
-  if (arg1) (arg1)->neighborJoint = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -53643,23 +53505,23 @@ int _wrap_Neighbor_neighborJoint_set(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_Neighbor_neighborJoint_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
+int _wrap_Box_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
+  iDynTree::SolidShape *result = 0 ;
   
-  if (!SWIG_check_num_args("Neighbor_neighborJoint_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Box_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborJoint_get" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_clone" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
-  result = (iDynTree::JointIndex) ((arg1)->neighborJoint);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  result = (iDynTree::SolidShape *)(arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53667,16 +53529,30 @@ int _wrap_Neighbor_neighborJoint_get(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_new_Neighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Box_x_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+  double arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Neighbor *result = 0 ;
   
-  if (!SWIG_check_num_args("new_Neighbor",argc,0,0,0)) {
+  if (!SWIG_check_num_args("Box_x_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::Neighbor *)new iDynTree::Neighbor();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Neighbor, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_x_set" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Box_x_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->x = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53684,26 +53560,23 @@ int _wrap_new_Neighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_Neighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
+int _wrap_Box_x_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  double result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_Neighbor",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Box_x_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Neighbor" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_x_get" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  result = (double) ((arg1)->x);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53711,16 +53584,30 @@ int _wrap_delete_Neighbor(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_new_Model__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Box_y_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+  double arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("new_Model",argc,0,0,0)) {
+  if (!SWIG_check_num_args("Box_y_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::Model *)new iDynTree::Model();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_y_set" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Box_y_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->y = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53728,26 +53615,23 @@ int _wrap_new_Model__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_new_Model__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_Box_y_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
+  double result;
   
-  if (!SWIG_check_num_args("new_Model",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Box_y_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_Model" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_Model" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_y_get" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::Model *)new iDynTree::Model((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  result = (double) ((arg1)->y);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53755,45 +53639,54 @@ int _wrap_new_Model__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_new_Model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_Model__SWIG_0(resc,resv,argc,argv);
+int _wrap_Box_z_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
+  double arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("Box_z_set",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_Model__SWIG_1(resc,resv,argc,argv);
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_z_set" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_Model'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::Model()\n"
-    "    iDynTree::Model::Model(iDynTree::Model const &)\n");
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Box_z_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->z = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_Model_copy(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_Box_z_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Box *arg1 = (iDynTree::Box *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Model result;
+  double result;
   
-  if (!SWIG_check_num_args("Model_copy",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Box_z_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Box, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_copy" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Box_z_get" "', argument " "1"" of type '" "iDynTree::Box *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->copy();
-  _out = SWIG_NewPointerObj((new iDynTree::Model(static_cast< const iDynTree::Model& >(result))), SWIGTYPE_p_iDynTree__Model, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::Box * >(argp1);
+  result = (double) ((arg1)->z);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53801,22 +53694,39 @@ int _wrap_Model_copy(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_delete_Model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_new_Box(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::Box *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_Box",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::Box *)new iDynTree::Box();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Box, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_Cylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_Model",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_Cylinder",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Model" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Cylinder" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -53828,23 +53738,23 @@ int _wrap_delete_Model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Model_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_Cylinder_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::SolidShape *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_getNrOfLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Cylinder_clone",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_clone" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->getNrOfLinks();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  result = (iDynTree::SolidShape *)(arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53852,31 +53762,30 @@ int _wrap_Model_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_Model_getLinkName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_Cylinder_length_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  double val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  std::string result;
   
-  if (!SWIG_check_num_args("Model_getLinkName",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Cylinder_length_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLinkName" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_length_set" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLinkName" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Cylinder_length_set" "', argument " "2"" of type '" "double""'");
   } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = ((iDynTree::Model const *)arg1)->getLinkName(arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->length = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53884,70 +53793,54 @@ int _wrap_Model_getLinkName(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_Model_getLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_Cylinder_length_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  double result;
   
-  if (!SWIG_check_num_args("Model_getLinkIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Cylinder_length_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLinkIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getLinkIndex" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getLinkIndex" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_length_get" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
   }
-  result = (iDynTree::LinkIndex)((iDynTree::Model const *)arg1)->getLinkIndex((std::string const &)*arg2);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  result = (double) ((arg1)->length);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Model_isValidLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_Cylinder_radius_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  double val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Model_isValidLinkIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Cylinder_radius_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isValidLinkIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_radius_set" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_isValidLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Cylinder_radius_set" "', argument " "2"" of type '" "double""'");
   } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (bool)((iDynTree::Model const *)arg1)->isValidLinkIndex(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->radius = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53955,31 +53848,23 @@ int _wrap_Model_isValidLinkIndex(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_Model_getLink__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_Cylinder_radius_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Cylinder *arg1 = (iDynTree::Cylinder *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkPtr result;
+  double result;
   
-  if (!SWIG_check_num_args("Model_getLink",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Cylinder_radius_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Cylinder, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Cylinder_radius_get" "', argument " "1"" of type '" "iDynTree::Cylinder *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::LinkPtr)(arg1)->getLink(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Cylinder * >(argp1);
+  result = (double) ((arg1)->radius);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -53987,31 +53872,43 @@ int _wrap_Model_getLink__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_Model_getLink__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_new_Cylinder(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::Cylinder *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_Cylinder",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::Cylinder *)new iDynTree::Cylinder();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Cylinder, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_ExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkConstPtr result;
   
-  if (!SWIG_check_num_args("Model_getLink",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ExternalMesh",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLink" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ExternalMesh" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (iDynTree::LinkConstPtr)((iDynTree::Model const *)arg1)->getLink(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54019,87 +53916,59 @@ int _wrap_Model_getLink__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_Model_getLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_Model_getLink__SWIG_0(resc,resv,argc,argv);
-      }
-    }
+int _wrap_ExternalMesh_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::SolidShape *result = 0 ;
+  
+  if (!SWIG_check_num_args("ExternalMesh_clone",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_Model_getLink__SWIG_1(resc,resv,argc,argv);
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_clone" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_getLink'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::getLink(iDynTree::LinkIndex const)\n"
-    "    iDynTree::Model::getLink(iDynTree::LinkIndex const) const\n");
+  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
+  result = (iDynTree::SolidShape *)(arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_Model_addLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_ExternalMesh_filename_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
   std::string *arg2 = 0 ;
-  iDynTree::Link *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int res2 = SWIG_OLDOBJ ;
-  void *argp3 ;
-  int res3 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Model_addLink",argc,3,3,0)) {
+  if (!SWIG_check_num_args("ExternalMesh_filename_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_filename_set" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
   {
     std::string *ptr = (std::string *)0;
     res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
     if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addLink" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExternalMesh_filename_set" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addLink" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExternalMesh_filename_set" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     arg2 = ptr;
   }
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Link,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addLink" "', argument " "3"" of type '" "iDynTree::Link const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addLink" "', argument " "3"" of type '" "iDynTree::Link const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Link * >(argp3);
-  result = (iDynTree::LinkIndex)(arg1)->addLink((std::string const &)*arg2,(iDynTree::Link const &)*arg3);
-  _out = SWIG_From_int(static_cast< int >(result));
+  if (arg1) (arg1)->filename = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
@@ -54109,23 +53978,23 @@ int _wrap_Model_addLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_Model_getNrOfJoints(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_ExternalMesh_filename_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  std::string *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_getNrOfJoints",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExternalMesh_filename_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfJoints" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_filename_get" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->getNrOfJoints();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
+  result = (std::string *) & ((arg1)->filename);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54133,31 +54002,30 @@ int _wrap_Model_getNrOfJoints(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_Model_getJointName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::JointIndex arg2 ;
+int _wrap_ExternalMesh_scale_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
+  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  std::string result;
   
-  if (!SWIG_check_num_args("Model_getJointName",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ExternalMesh_scale_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJointName" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_scale_set" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getJointName" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::JointIndex >(val2);
-  result = ((iDynTree::Model const *)arg1)->getJointName(arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExternalMesh_scale_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
+  if (arg1) (arg1)->scale = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54165,70 +54033,57 @@ int _wrap_Model_getJointName(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_Model_getJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_ExternalMesh_scale_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExternalMesh *arg1 = (iDynTree::ExternalMesh *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
-  iDynTree::JointIndex result;
+  iDynTree::Vector3 *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_getJointIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ExternalMesh_scale_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExternalMesh, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJointIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getJointIndex" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getJointIndex" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExternalMesh_scale_get" "', argument " "1"" of type '" "iDynTree::ExternalMesh *""'"); 
   }
-  result = (iDynTree::JointIndex)((iDynTree::Model const *)arg1)->getJointIndex((std::string const &)*arg2);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::ExternalMesh * >(argp1);
+  result = (iDynTree::Vector3 *)& ((arg1)->scale);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Model_getJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::JointIndex arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+int _wrap_new_ExternalMesh(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::IJointPtr result;
+  iDynTree::ExternalMesh *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_getJoint",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_ExternalMesh",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJoint" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+  (void)argv;
+  result = (iDynTree::ExternalMesh *)new iDynTree::ExternalMesh();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExternalMesh, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_ModelSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::ModelSolidShapes *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_ModelSolidShapes",argc,0,0,0)) {
+    SWIG_fail;
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getJoint" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::JointIndex >(val2);
-  result = (iDynTree::IJointPtr)(arg1)->getJoint(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  (void)argv;
+  result = (iDynTree::ModelSolidShapes *)new iDynTree::ModelSolidShapes();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54236,31 +54091,26 @@ int _wrap_Model_getJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_Model_getJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::JointIndex arg2 ;
-  void *argp1 = 0 ;
+int _wrap_new_ModelSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::IJointConstPtr result;
+  iDynTree::ModelSolidShapes *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_getJoint",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_ModelSolidShapes",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__ModelSolidShapes,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJoint" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_ModelSolidShapes" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getJoint" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::JointIndex >(val2);
-  result = (iDynTree::IJointConstPtr)((iDynTree::Model const *)arg1)->getJoint(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_ModelSolidShapes" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  result = (iDynTree::ModelSolidShapes *)new iDynTree::ModelSolidShapes((iDynTree::ModelSolidShapes const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54268,71 +54118,44 @@ int _wrap_Model_getJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_Model_getJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_Model_getJoint__SWIG_0(resc,resv,argc,argv);
-      }
-    }
+int _wrap_new_ModelSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_ModelSolidShapes__SWIG_0(resc,resv,argc,argv);
   }
-  if (argc == 2) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelSolidShapes, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_Model_getJoint__SWIG_1(resc,resv,argc,argv);
-      }
+      return _wrap_new_ModelSolidShapes__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_getJoint'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_ModelSolidShapes'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::getJoint(iDynTree::JointIndex const)\n"
-    "    iDynTree::Model::getJoint(iDynTree::JointIndex const) const\n");
+    "    iDynTree::ModelSolidShapes::ModelSolidShapes()\n"
+    "    iDynTree::ModelSolidShapes::ModelSolidShapes(iDynTree::ModelSolidShapes const &)\n");
   return 1;
 }
 
 
-int _wrap_Model_isValidJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::JointIndex arg2 ;
+int _wrap_ModelSolidShapes_clear(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Model_isValidJointIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ModelSolidShapes_clear",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isValidJointIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_clear" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_isValidJointIndex" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::JointIndex >(val2);
-  result = (bool)((iDynTree::Model const *)arg1)->isValidJointIndex(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  (arg1)->clear();
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54340,33 +54163,26 @@ int _wrap_Model_isValidJointIndex(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_Model_isLinkNameUsed(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string arg2 ;
+int _wrap_delete_ModelSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Model_isLinkNameUsed",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ModelSolidShapes",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isLinkNameUsed" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelSolidShapes" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "Model_isLinkNameUsed" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  result = (bool)((iDynTree::Model const *)arg1)->isLinkNameUsed(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54374,33 +54190,30 @@ int _wrap_Model_isLinkNameUsed(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_Model_isJointNameUsed(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string arg2 ;
+int _wrap_ModelSolidShapes_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Model_isJointNameUsed",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ModelSolidShapes_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isJointNameUsed" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "Model_isJointNameUsed" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_resize" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
   }
-  result = (bool)((iDynTree::Model const *)arg1)->isJointNameUsed(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ModelSolidShapes_resize" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->resize(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54408,437 +54221,193 @@ int _wrap_Model_isJointNameUsed(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_Model_isFrameNameUsed(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string arg2 ;
+int _wrap_ModelSolidShapes_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("Model_isFrameNameUsed",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ModelSolidShapes_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isFrameNameUsed" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "Model_isFrameNameUsed" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  result = (bool)((iDynTree::Model const *)arg1)->isFrameNameUsed(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_Model_addJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
-  iDynTree::IJointConstPtr arg3 = (iDynTree::IJointConstPtr) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  mxArray * _out;
-  iDynTree::JointIndex result;
-  
-  if (!SWIG_check_num_args("Model_addJoint",argc,3,3,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addJoint" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
-  }
-  res3 = SWIG_ConvertPtr(argv[2], &argp3,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addJoint" "', argument " "3"" of type '" "iDynTree::IJointConstPtr""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::IJointConstPtr >(argp3);
-  result = (iDynTree::JointIndex)(arg1)->addJoint((std::string const &)*arg2,arg3);
-  _out = SWIG_From_int(static_cast< int >(result));
-  if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  return 0;
-fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  return 1;
-}
-
-
-int _wrap_Model_addJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
-  std::string *arg4 = 0 ;
-  iDynTree::IJointConstPtr arg5 = (iDynTree::IJointConstPtr) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
-  int res4 = SWIG_OLDOBJ ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  mxArray * _out;
-  iDynTree::JointIndex result;
-  
-  if (!SWIG_check_num_args("Model_addJoint",argc,5,5,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addJoint" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addJoint" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    arg3 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_resize" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res4 = SWIG_AsPtr_std_string(argv[3], &ptr);
-    if (!SWIG_IsOK(res4)) {
-      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_addJoint" "', argument " "4"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "4"" of type '" "std::string const &""'"); 
-    }
-    arg4 = ptr;
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelSolidShapes_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  res5 = SWIG_ConvertPtr(argv[4], &argp5,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "Model_addJoint" "', argument " "5"" of type '" "iDynTree::IJointConstPtr""'"); 
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelSolidShapes_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::IJointConstPtr >(argp5);
-  result = (iDynTree::JointIndex)(arg1)->addJoint((std::string const &)*arg2,(std::string const &)*arg3,(std::string const &)*arg4,arg5);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  if (SWIG_IsNewObj(res4)) delete arg4;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  if (SWIG_IsNewObj(res4)) delete arg4;
   return 1;
 }
 
 
-int _wrap_Model_addJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
+int _wrap_ModelSolidShapes_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelSolidShapes, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__IJoint, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_Model_addJoint__SWIG_0(resc,resv,argc,argv);
-        }
+        return _wrap_ModelSolidShapes_resize__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 5) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelSolidShapes, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
         _v = SWIG_CheckState(res);
-        if (_v) {
-          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            void *vptr = 0;
-            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__IJoint, 0);
-            _v = SWIG_CheckState(res);
-            if (_v) {
-              return _wrap_Model_addJoint__SWIG_1(resc,resv,argc,argv);
-            }
-          }
-        }
+      }
+      if (_v) {
+        return _wrap_ModelSolidShapes_resize__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_addJoint'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelSolidShapes_resize'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::addJoint(std::string const &,iDynTree::IJointConstPtr)\n"
-    "    iDynTree::Model::addJoint(std::string const &,std::string const &,std::string const &,iDynTree::IJointConstPtr)\n");
+    "    iDynTree::ModelSolidShapes::resize(size_t)\n"
+    "    iDynTree::ModelSolidShapes::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_Model_addJointAndLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
-  iDynTree::IJointConstPtr arg4 = (iDynTree::IJointConstPtr) 0 ;
-  std::string *arg5 = 0 ;
-  iDynTree::Link *arg6 = 0 ;
+int _wrap_ModelSolidShapes_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  int res5 = SWIG_OLDOBJ ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
+  bool result;
   
-  if (!SWIG_check_num_args("Model_addJointAndLink",argc,6,6,0)) {
+  if (!SWIG_check_num_args("ModelSolidShapes_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addJointAndLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addJointAndLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addJointAndLink" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  res4 = SWIG_ConvertPtr(argv[3], &argp4,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_addJointAndLink" "', argument " "4"" of type '" "iDynTree::IJointConstPtr""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::IJointConstPtr >(argp4);
-  {
-    std::string *ptr = (std::string *)0;
-    res5 = SWIG_AsPtr_std_string(argv[4], &ptr);
-    if (!SWIG_IsOK(res5)) {
-      SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "Model_addJointAndLink" "', argument " "5"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "5"" of type '" "std::string const &""'"); 
-    }
-    arg5 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_isConsistent" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes const *""'"); 
   }
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__Link,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "Model_addJointAndLink" "', argument " "6"" of type '" "iDynTree::Link &""'"); 
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelSolidShapes_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "6"" of type '" "iDynTree::Link &""'"); 
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelSolidShapes_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::Link * >(argp6);
-  result = (iDynTree::JointIndex)(arg1)->addJointAndLink((std::string const &)*arg2,(std::string const &)*arg3,arg4,(std::string const &)*arg5,*arg6);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::ModelSolidShapes const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  if (SWIG_IsNewObj(res5)) delete arg5;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  if (SWIG_IsNewObj(res5)) delete arg5;
   return 1;
 }
 
 
-int _wrap_Model_insertLinkToExistingJointAndAddJointForDisplacedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
-  iDynTree::Transform *arg4 = 0 ;
-  std::string *arg5 = 0 ;
-  iDynTree::IJointConstPtr arg6 = (iDynTree::IJointConstPtr) 0 ;
-  std::string *arg7 = 0 ;
-  iDynTree::Link *arg8 = 0 ;
+int _wrap_ModelSolidShapes_linkSolidShapes_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+  std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *arg2 = (std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
-  void *argp4 ;
-  int res4 = 0 ;
-  int res5 = SWIG_OLDOBJ ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  int res7 = SWIG_OLDOBJ ;
-  void *argp8 = 0 ;
-  int res8 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("Model_insertLinkToExistingJointAndAddJointForDisplacedLink",argc,8,8,0)) {
+  if (!SWIG_check_num_args("ModelSolidShapes_linkSolidShapes_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "4"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "4"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::Transform * >(argp4);
-  {
-    std::string *ptr = (std::string *)0;
-    res5 = SWIG_AsPtr_std_string(argv[4], &ptr);
-    if (!SWIG_IsOK(res5)) {
-      SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "5"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "5"" of type '" "std::string const &""'"); 
-    }
-    arg5 = ptr;
-  }
-  res6 = SWIG_ConvertPtr(argv[5], &argp6,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "6"" of type '" "iDynTree::IJointConstPtr""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_linkSolidShapes_set" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::IJointConstPtr >(argp6);
-  {
-    std::string *ptr = (std::string *)0;
-    res7 = SWIG_AsPtr_std_string(argv[6], &ptr);
-    if (!SWIG_IsOK(res7)) {
-      SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "7"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "7"" of type '" "std::string const &""'"); 
-    }
-    arg7 = ptr;
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelSolidShapes_linkSolidShapes_set" "', argument " "2"" of type '" "std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *""'"); 
   }
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__Link,  0 );
-  if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "8"" of type '" "iDynTree::Link &""'"); 
+  arg2 = reinterpret_cast< std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > * >(argp2);
+  if (arg1) (arg1)->linkSolidShapes = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelSolidShapes_linkSolidShapes_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelSolidShapes *arg1 = (iDynTree::ModelSolidShapes *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelSolidShapes_linkSolidShapes_get",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "8"" of type '" "iDynTree::Link &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelSolidShapes_linkSolidShapes_get" "', argument " "1"" of type '" "iDynTree::ModelSolidShapes *""'"); 
   }
-  arg8 = reinterpret_cast< iDynTree::Link * >(argp8);
-  result = (iDynTree::JointIndex)(arg1)->insertLinkToExistingJointAndAddJointForDisplacedLink((std::string const &)*arg2,(std::string const &)*arg3,(iDynTree::Transform const &)*arg4,(std::string const &)*arg5,arg6,(std::string const &)*arg7,*arg8);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::ModelSolidShapes * >(argp1);
+  result = (std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *)& ((arg1)->linkSolidShapes);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  if (SWIG_IsNewObj(res5)) delete arg5;
-  if (SWIG_IsNewObj(res7)) delete arg7;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  if (SWIG_IsNewObj(res5)) delete arg5;
-  if (SWIG_IsNewObj(res7)) delete arg7;
   return 1;
 }
 
 
-int _wrap_Model_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_Neighbor_neighborLink_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  size_t result;
   
-  if (!SWIG_check_num_args("Model_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Neighbor_neighborLink_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborLink_set" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Neighbor_neighborLink_set" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  if (arg1) (arg1)->neighborLink = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54846,23 +54415,23 @@ int _wrap_Model_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_Model_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_Neighbor_neighborLink_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Model_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Neighbor_neighborLink_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborLink_get" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
+  result = (iDynTree::LinkIndex) ((arg1)->neighborLink);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54870,23 +54439,30 @@ int _wrap_Model_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_Model_getNrOfFrames(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+int _wrap_Neighbor_neighborJoint_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
+  iDynTree::JointIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  size_t result;
   
-  if (!SWIG_check_num_args("Model_getNrOfFrames",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Neighbor_neighborJoint_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfFrames" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborJoint_set" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->getNrOfFrames();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Neighbor_neighborJoint_set" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::JointIndex >(val2);
+  if (arg1) (arg1)->neighborJoint = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54894,99 +54470,40 @@ int _wrap_Model_getNrOfFrames(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_Model_addAdditionalFrameToLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
-  iDynTree::Transform arg4 ;
+int _wrap_Neighbor_neighborJoint_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
-  void *argp4 ;
-  int res4 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("Model_addAdditionalFrameToLink",argc,4,4,0)) {
+  if (!SWIG_check_num_args("Neighbor_neighborJoint_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addAdditionalFrameToLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addAdditionalFrameToLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addAdditionalFrameToLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Neighbor_neighborJoint_get" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addAdditionalFrameToLink" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addAdditionalFrameToLink" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  {
-    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
-    if (!SWIG_IsOK(res4)) {
-      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_addAdditionalFrameToLink" "', argument " "4"" of type '" "iDynTree::Transform""'"); 
-    }  
-    if (!argp4) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addAdditionalFrameToLink" "', argument " "4"" of type '" "iDynTree::Transform""'");
-    } else {
-      arg4 = *(reinterpret_cast< iDynTree::Transform * >(argp4));
-    }
-  }
-  result = (bool)(arg1)->addAdditionalFrameToLink((std::string const &)*arg2,(std::string const &)*arg3,arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
+  result = (iDynTree::JointIndex) ((arg1)->neighborJoint);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_Model_getFrameName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::FrameIndex arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+int _wrap_new_Neighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  std::string result;
+  iDynTree::Neighbor *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_getFrameName",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_Neighbor",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameName" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getFrameName" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  result = ((iDynTree::Model const *)arg1)->getFrameName(arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  (void)argv;
+  result = (iDynTree::Neighbor *)new iDynTree::Neighbor();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Neighbor, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -54994,70 +54511,70 @@ int _wrap_Model_getFrameName(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_Model_getFrameIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_delete_Neighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Neighbor *arg1 = (iDynTree::Neighbor *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
-  iDynTree::FrameIndex result;
   
-  if (!SWIG_check_num_args("Model_getFrameIndex",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_Neighbor",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Neighbor, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Neighbor" "', argument " "1"" of type '" "iDynTree::Neighbor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getFrameIndex" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getFrameIndex" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+  arg1 = reinterpret_cast< iDynTree::Neighbor * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  result = (iDynTree::FrameIndex)((iDynTree::Model const *)arg1)->getFrameIndex((std::string const &)*arg2);
-  _out = SWIG_From_int(static_cast< int >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Model_isValidFrameIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::FrameIndex arg2 ;
-  void *argp1 = 0 ;
+int _wrap_new_Model__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::Model *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_Model",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::Model *)new iDynTree::Model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_Model__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("Model_isValidFrameIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_Model",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isValidFrameIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_Model" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_Model" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_isValidFrameIndex" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  result = (bool)((iDynTree::Model const *)arg1)->isValidFrameIndex(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  result = (iDynTree::Model *)new iDynTree::Model((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55065,31 +54582,45 @@ int _wrap_Model_isValidFrameIndex(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_Model_getFrameTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_Model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_Model__SWIG_0(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_Model__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_Model'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::Model::Model()\n"
+    "    iDynTree::Model::Model(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_Model_copy(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Transform result;
+  iDynTree::Model result;
   
-  if (!SWIG_check_num_args("Model_getFrameTransform",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_copy",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameTransform" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_copy" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getFrameTransform" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  result = ((iDynTree::Model const *)arg1)->getFrameTransform(arg2);
-  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  result = ((iDynTree::Model const *)arg1)->copy();
+  _out = SWIG_NewPointerObj((new iDynTree::Model(static_cast< const iDynTree::Model& >(result))), SWIGTYPE_p_iDynTree__Model, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55097,31 +54628,26 @@ int _wrap_Model_getFrameTransform(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_Model_getFrameLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_delete_Model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Model_getFrameLink",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_Model",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameLink" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Model" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getFrameLink" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  result = (iDynTree::LinkIndex)((iDynTree::Model const *)arg1)->getFrameLink(arg2);
-  _out = SWIG_From_int(static_cast< int >(result));
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55129,31 +54655,23 @@ int _wrap_Model_getFrameLink(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_Model_getNrOfNeighbors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  size_t result;
   
-  if (!SWIG_check_num_args("Model_getNrOfNeighbors",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_getNrOfLinks",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfNeighbors" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getNrOfNeighbors" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (unsigned int)((iDynTree::Model const *)arg1)->getNrOfNeighbors(arg2);
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  result = ((iDynTree::Model const *)arg1)->getNrOfLinks();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55161,39 +54679,31 @@ int _wrap_Model_getNrOfNeighbors(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_Model_getNeighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getLinkName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
   iDynTree::LinkIndex arg2 ;
-  unsigned int arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int val2 ;
   int ecode2 = 0 ;
-  unsigned int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::Neighbor result;
+  std::string result;
   
-  if (!SWIG_check_num_args("Model_getNeighbor",argc,3,3,0)) {
+  if (!SWIG_check_num_args("Model_getLinkName",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNeighbor" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLinkName" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
   ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getNeighbor" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLinkName" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
   arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_unsigned_SS_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Model_getNeighbor" "', argument " "3"" of type '" "unsigned int""'");
-  } 
-  arg3 = static_cast< unsigned int >(val3);
-  result = ((iDynTree::Model const *)arg1)->getNeighbor(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::Neighbor(static_cast< const iDynTree::Neighbor& >(result))), SWIGTYPE_p_iDynTree__Neighbor, SWIG_POINTER_OWN |  0 );
+  result = ((iDynTree::Model const *)arg1)->getLinkName(arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55201,55 +54711,70 @@ int _wrap_Model_getNeighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_Model_setDefaultBaseLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Model_setDefaultBaseLink",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_getLinkIndex",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_setDefaultBaseLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLinkIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_setDefaultBaseLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (bool)(arg1)->setDefaultBaseLink(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getLinkIndex" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getLinkIndex" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (iDynTree::LinkIndex)((iDynTree::Model const *)arg1)->getLinkIndex((std::string const &)*arg2);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Model_getDefaultBaseLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_isValidLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  bool result;
   
-  if (!SWIG_check_num_args("Model_getDefaultBaseLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_isValidLinkIndex",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getDefaultBaseLink" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isValidLinkIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::Model const *)arg1)->getDefaultBaseLink();
-  _out = SWIG_From_int(static_cast< int >(result));
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_isValidLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (bool)((iDynTree::Model const *)arg1)->isValidLinkIndex(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55257,34 +54782,31 @@ int _wrap_Model_getDefaultBaseLink(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_Model_computeFullTreeTraversal__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getLink__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkPtr result;
   
-  if (!SWIG_check_num_args("Model_computeFullTreeTraversal",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_getLink",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_computeFullTreeTraversal" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  result = (bool)((iDynTree::Model const *)arg1)->computeFullTreeTraversal(*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::LinkPtr)(arg1)->getLink(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55292,42 +54814,31 @@ int _wrap_Model_computeFullTreeTraversal__SWIG_0(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_Model_computeFullTreeTraversal__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getLink__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::LinkIndex arg3 ;
+  iDynTree::LinkIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkConstPtr result;
   
-  if (!SWIG_check_num_args("Model_computeFullTreeTraversal",argc,3,3,0)) {
+  if (!SWIG_check_num_args("Model_getLink",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_computeFullTreeTraversal" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLink" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Model_computeFullTreeTraversal" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
   } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = (bool)((iDynTree::Model const *)arg1)->computeFullTreeTraversal(*arg2,arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (iDynTree::LinkConstPtr)((iDynTree::Model const *)arg1)->getLink(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Link, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55335,113 +54846,113 @@ int _wrap_Model_computeFullTreeTraversal__SWIG_1(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_Model_computeFullTreeTraversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
-      _v = SWIG_CheckState(res);
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
       if (_v) {
-        return _wrap_Model_computeFullTreeTraversal__SWIG_0(resc,resv,argc,argv);
+        return _wrap_Model_getLink__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 3) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
-      _v = SWIG_CheckState(res);
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
       if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_Model_computeFullTreeTraversal__SWIG_1(resc,resv,argc,argv);
-        }
+        return _wrap_Model_getLink__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_computeFullTreeTraversal'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_getLink'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::computeFullTreeTraversal(iDynTree::Traversal &) const\n"
-    "    iDynTree::Model::computeFullTreeTraversal(iDynTree::Traversal &,iDynTree::LinkIndex const) const\n");
+    "    iDynTree::Model::getLink(iDynTree::LinkIndex const)\n"
+    "    iDynTree::Model::getLink(iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_Model_getInertialParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_addLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::Link *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("Model_getInertialParameters",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_addLink",argc,3,3,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getInertialParameters" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Link,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addLink" "', argument " "3"" of type '" "iDynTree::Link const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  result = (bool)((iDynTree::Model const *)arg1)->getInertialParameters(*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addLink" "', argument " "3"" of type '" "iDynTree::Link const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Link * >(argp3);
+  result = (iDynTree::LinkIndex)(arg1)->addLink((std::string const &)*arg2,(iDynTree::Link const &)*arg3);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Model_updateInertialParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getNrOfJoints(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("Model_updateInertialParameters",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_getNrOfJoints",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_updateInertialParameters" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfJoints" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_updateInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_updateInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  result = (bool)(arg1)->updateInertialParameters((iDynTree::VectorDynSize const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  result = ((iDynTree::Model const *)arg1)->getNrOfJoints();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55449,23 +54960,31 @@ int _wrap_Model_updateInertialParameters(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_Model_visualSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getJointName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::JointIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::ModelSolidShapes *result = 0 ;
+  std::string result;
   
-  if (!SWIG_check_num_args("Model_visualSolidShapes",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getJointName",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_visualSolidShapes" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJointName" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::ModelSolidShapes *) &(arg1)->visualSolidShapes();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getJointName" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::JointIndex >(val2);
+  result = ((iDynTree::Model const *)arg1)->getJointName(arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55473,75 +54992,70 @@ int _wrap_Model_visualSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_Model_visualSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
-  iDynTree::ModelSolidShapes *result = 0 ;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("Model_visualSolidShapes",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getJointIndex",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_visualSolidShapes" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJointIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::ModelSolidShapes *) &((iDynTree::Model const *)arg1)->visualSolidShapes();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_Model_visualSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_Model_visualSolidShapes__SWIG_0(resc,resv,argc,argv);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getJointIndex" "', argument " "2"" of type '" "std::string const &""'"); 
     }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_Model_visualSolidShapes__SWIG_1(resc,resv,argc,argv);
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getJointIndex" "', argument " "2"" of type '" "std::string const &""'"); 
     }
+    arg2 = ptr;
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_visualSolidShapes'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::visualSolidShapes()\n"
-    "    iDynTree::Model::visualSolidShapes() const\n");
+  result = (iDynTree::JointIndex)((iDynTree::Model const *)arg1)->getJointIndex((std::string const &)*arg2);
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_Model_collisionSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::JointIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::ModelSolidShapes *result = 0 ;
+  iDynTree::IJointPtr result;
   
-  if (!SWIG_check_num_args("Model_collisionSolidShapes",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getJoint",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_collisionSolidShapes" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJoint" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::ModelSolidShapes *) &(arg1)->collisionSolidShapes();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getJoint" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::JointIndex >(val2);
+  result = (iDynTree::IJointPtr)(arg1)->getJoint(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55549,23 +55063,31 @@ int _wrap_Model_collisionSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_Model_collisionSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::JointIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::ModelSolidShapes *result = 0 ;
+  iDynTree::IJointConstPtr result;
   
-  if (!SWIG_check_num_args("Model_collisionSolidShapes",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getJoint",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_collisionSolidShapes" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getJoint" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::ModelSolidShapes *) &((iDynTree::Model const *)arg1)->collisionSolidShapes();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getJoint" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::JointIndex >(val2);
+  result = (iDynTree::IJointConstPtr)((iDynTree::Model const *)arg1)->getJoint(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55573,51 +55095,71 @@ int _wrap_Model_collisionSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_Model_collisionSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
+int _wrap_Model_getJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_Model_collisionSolidShapes__SWIG_0(resc,resv,argc,argv);
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_Model_getJoint__SWIG_0(resc,resv,argc,argv);
+      }
     }
   }
-  if (argc == 1) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_Model_collisionSolidShapes__SWIG_1(resc,resv,argc,argv);
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_Model_getJoint__SWIG_1(resc,resv,argc,argv);
+      }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_collisionSolidShapes'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_getJoint'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::Model::collisionSolidShapes()\n"
-    "    iDynTree::Model::collisionSolidShapes() const\n");
+    "    iDynTree::Model::getJoint(iDynTree::JointIndex const)\n"
+    "    iDynTree::Model::getJoint(iDynTree::JointIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_Model_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_isValidJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::JointIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  std::string result;
+  bool result;
   
-  if (!SWIG_check_num_args("Model_toString",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_isValidJointIndex",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_toString" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isValidJointIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = ((iDynTree::Model const *)arg1)->toString();
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_isValidJointIndex" "', argument " "2"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::JointIndex >(val2);
+  result = (bool)((iDynTree::Model const *)arg1)->isValidJointIndex(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55625,23 +55167,33 @@ int _wrap_Model_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_new_JointPosDoubleArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
+int _wrap_Model_isLinkNameUsed(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointPosDoubleArray *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_JointPosDoubleArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_isLinkNameUsed",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_JointPosDoubleArray" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::JointPosDoubleArray *)new iDynTree::JointPosDoubleArray(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isLinkNameUsed" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "Model_isLinkNameUsed" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)((iDynTree::Model const *)arg1)->isLinkNameUsed(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55649,16 +55201,33 @@ int _wrap_new_JointPosDoubleArray__SWIG_0(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_new_JointPosDoubleArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_isJointNameUsed(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointPosDoubleArray *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_JointPosDoubleArray",argc,0,0,0)) {
+  if (!SWIG_check_num_args("Model_isJointNameUsed",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::JointPosDoubleArray *)new iDynTree::JointPosDoubleArray();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isJointNameUsed" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "Model_isJointNameUsed" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)((iDynTree::Model const *)arg1)->isJointNameUsed(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55666,26 +55235,33 @@ int _wrap_new_JointPosDoubleArray__SWIG_1(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_new_JointPosDoubleArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_Model_isFrameNameUsed(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string arg2 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointPosDoubleArray *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_JointPosDoubleArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_isFrameNameUsed",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_JointPosDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_JointPosDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isFrameNameUsed" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::JointPosDoubleArray *)new iDynTree::JointPosDoubleArray((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 1 |  0 );
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "Model_isFrameNameUsed" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)((iDynTree::Model const *)arg1)->isFrameNameUsed(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55693,222 +55269,403 @@ int _wrap_new_JointPosDoubleArray__SWIG_2(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_new_JointPosDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_JointPosDoubleArray__SWIG_1(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_JointPosDoubleArray__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_JointPosDoubleArray__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_JointPosDoubleArray'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::JointPosDoubleArray::JointPosDoubleArray(unsigned int)\n"
-    "    iDynTree::JointPosDoubleArray::JointPosDoubleArray()\n"
-    "    iDynTree::JointPosDoubleArray::JointPosDoubleArray(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_JointPosDoubleArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
-  unsigned int arg2 ;
+int _wrap_Model_addJoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::IJointConstPtr arg3 = (iDynTree::IJointConstPtr) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
   mxArray * _out;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("JointPosDoubleArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_addJoint",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointPosDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addJoint" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "JointPosDoubleArray_resize" "', argument " "2"" of type '" "unsigned int""'");
-  } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  res3 = SWIG_ConvertPtr(argv[2], &argp3,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addJoint" "', argument " "3"" of type '" "iDynTree::IJointConstPtr""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::IJointConstPtr >(argp3);
+  result = (iDynTree::JointIndex)(arg1)->addJoint((std::string const &)*arg2,arg3);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_JointPosDoubleArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_Model_addJoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  std::string *arg4 = 0 ;
+  iDynTree::IJointConstPtr arg5 = (iDynTree::IJointConstPtr) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  int res4 = SWIG_OLDOBJ ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("JointPosDoubleArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_addJoint",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointPosDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addJoint" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointPosDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointPosDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addJoint" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  {
+    std::string *ptr = (std::string *)0;
+    res4 = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res4)) {
+      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_addJoint" "', argument " "4"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJoint" "', argument " "4"" of type '" "std::string const &""'"); 
+    }
+    arg4 = ptr;
+  }
+  res5 = SWIG_ConvertPtr(argv[4], &argp5,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "Model_addJoint" "', argument " "5"" of type '" "iDynTree::IJointConstPtr""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::IJointConstPtr >(argp5);
+  result = (iDynTree::JointIndex)(arg1)->addJoint((std::string const &)*arg2,(std::string const &)*arg3,(std::string const &)*arg4,arg5);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  if (SWIG_IsNewObj(res4)) delete arg4;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  if (SWIG_IsNewObj(res4)) delete arg4;
   return 1;
 }
 
 
-int _wrap_JointPosDoubleArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_Model_addJoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_JointPosDoubleArray_resize__SWIG_1(resc,resv,argc,argv);
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__IJoint, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_Model_addJoint__SWIG_0(resc,resv,argc,argv);
+        }
       }
     }
   }
-  if (argc == 2) {
+  if (argc == 5) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_JointPosDoubleArray_resize__SWIG_0(resc,resv,argc,argv);
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__IJoint, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_Model_addJoint__SWIG_1(resc,resv,argc,argv);
+            }
+          }
+        }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'JointPosDoubleArray_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_addJoint'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::JointPosDoubleArray::resize(unsigned int)\n"
-    "    iDynTree::JointPosDoubleArray::resize(iDynTree::Model const &)\n");
+    "    iDynTree::Model::addJoint(std::string const &,iDynTree::IJointConstPtr)\n"
+    "    iDynTree::Model::addJoint(std::string const &,std::string const &,std::string const &,iDynTree::IJointConstPtr)\n");
   return 1;
 }
 
 
-int _wrap_JointPosDoubleArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_Model_addJointAndLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  iDynTree::IJointConstPtr arg4 = (iDynTree::IJointConstPtr) 0 ;
+  std::string *arg5 = 0 ;
+  iDynTree::Link *arg6 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  int res5 = SWIG_OLDOBJ ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::JointIndex result;
   
-  if (!SWIG_check_num_args("JointPosDoubleArray_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_addJointAndLink",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointPosDoubleArray_isConsistent" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addJointAndLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointPosDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addJointAndLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointPosDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addJointAndLink" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::JointPosDoubleArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  res4 = SWIG_ConvertPtr(argv[3], &argp4,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_addJointAndLink" "', argument " "4"" of type '" "iDynTree::IJointConstPtr""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::IJointConstPtr >(argp4);
+  {
+    std::string *ptr = (std::string *)0;
+    res5 = SWIG_AsPtr_std_string(argv[4], &ptr);
+    if (!SWIG_IsOK(res5)) {
+      SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "Model_addJointAndLink" "', argument " "5"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "5"" of type '" "std::string const &""'"); 
+    }
+    arg5 = ptr;
+  }
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__Link,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "Model_addJointAndLink" "', argument " "6"" of type '" "iDynTree::Link &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addJointAndLink" "', argument " "6"" of type '" "iDynTree::Link &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::Link * >(argp6);
+  result = (iDynTree::JointIndex)(arg1)->addJointAndLink((std::string const &)*arg2,(std::string const &)*arg3,arg4,(std::string const &)*arg5,*arg6);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  if (SWIG_IsNewObj(res5)) delete arg5;
   return 0;
 fail:
-  return 1;
-}
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  if (SWIG_IsNewObj(res5)) delete arg5;
+  return 1;
+}
 
 
-int _wrap_delete_JointPosDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
+int _wrap_Model_insertLinkToExistingJointAndAddJointForDisplacedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  iDynTree::Transform *arg4 = 0 ;
+  std::string *arg5 = 0 ;
+  iDynTree::IJointConstPtr arg6 = (iDynTree::IJointConstPtr) 0 ;
+  std::string *arg7 = 0 ;
+  iDynTree::Link *arg8 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  void *argp4 ;
+  int res4 = 0 ;
+  int res5 = SWIG_OLDOBJ ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  int res7 = SWIG_OLDOBJ ;
+  void *argp8 = 0 ;
+  int res8 = 0 ;
   mxArray * _out;
+  iDynTree::JointIndex result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_JointPosDoubleArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_insertLinkToExistingJointAndAddJointForDisplacedLink",argc,8,8,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_JointPosDoubleArray" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  _out = (mxArray*)0;
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "4"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "4"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Transform * >(argp4);
+  {
+    std::string *ptr = (std::string *)0;
+    res5 = SWIG_AsPtr_std_string(argv[4], &ptr);
+    if (!SWIG_IsOK(res5)) {
+      SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "5"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "5"" of type '" "std::string const &""'"); 
+    }
+    arg5 = ptr;
+  }
+  res6 = SWIG_ConvertPtr(argv[5], &argp6,SWIGTYPE_p_iDynTree__IJoint, 0 |  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "6"" of type '" "iDynTree::IJointConstPtr""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::IJointConstPtr >(argp6);
+  {
+    std::string *ptr = (std::string *)0;
+    res7 = SWIG_AsPtr_std_string(argv[6], &ptr);
+    if (!SWIG_IsOK(res7)) {
+      SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "7"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "7"" of type '" "std::string const &""'"); 
+    }
+    arg7 = ptr;
+  }
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__Link,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "8"" of type '" "iDynTree::Link &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_insertLinkToExistingJointAndAddJointForDisplacedLink" "', argument " "8"" of type '" "iDynTree::Link &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::Link * >(argp8);
+  result = (iDynTree::JointIndex)(arg1)->insertLinkToExistingJointAndAddJointForDisplacedLink((std::string const &)*arg2,(std::string const &)*arg3,(iDynTree::Transform const &)*arg4,(std::string const &)*arg5,arg6,(std::string const &)*arg7,*arg8);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  if (SWIG_IsNewObj(res5)) delete arg5;
+  if (SWIG_IsNewObj(res7)) delete arg7;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  if (SWIG_IsNewObj(res5)) delete arg5;
+  if (SWIG_IsNewObj(res7)) delete arg7;
   return 1;
 }
 
 
-int _wrap_new_JointDOFsDoubleArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
+int _wrap_Model_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  size_t result;
   
-  if (!SWIG_check_num_args("new_JointDOFsDoubleArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_JointDOFsDoubleArray" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::JointDOFsDoubleArray *)new iDynTree::JointDOFsDoubleArray(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = ((iDynTree::Model const *)arg1)->getNrOfPosCoords();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55916,16 +55673,23 @@ int _wrap_new_JointDOFsDoubleArray__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_JointDOFsDoubleArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  size_t result;
   
-  if (!SWIG_check_num_args("new_JointDOFsDoubleArray",argc,0,0,0)) {
+  if (!SWIG_check_num_args("Model_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::JointDOFsDoubleArray *)new iDynTree::JointDOFsDoubleArray();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = ((iDynTree::Model const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55933,26 +55697,23 @@ int _wrap_new_JointDOFsDoubleArray__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_JointDOFsDoubleArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_Model_getNrOfFrames(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  size_t result;
   
-  if (!SWIG_check_num_args("new_JointDOFsDoubleArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getNrOfFrames",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_JointDOFsDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_JointDOFsDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfFrames" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *)new iDynTree::JointDOFsDoubleArray((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 1 |  0 );
+  result = ((iDynTree::Model const *)arg1)->getNrOfFrames();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -55960,63 +55721,99 @@ int _wrap_new_JointDOFsDoubleArray__SWIG_2(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_JointDOFsDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_JointDOFsDoubleArray__SWIG_1(resc,resv,argc,argv);
+int _wrap_Model_addAdditionalFrameToLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  iDynTree::Transform arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("Model_addAdditionalFrameToLink",argc,4,4,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_JointDOFsDoubleArray__SWIG_2(resc,resv,argc,argv);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_addAdditionalFrameToLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_addAdditionalFrameToLink" "', argument " "2"" of type '" "std::string const &""'"); 
     }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addAdditionalFrameToLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_addAdditionalFrameToLink" "', argument " "3"" of type '" "std::string const &""'"); 
     }
-    if (_v) {
-      return _wrap_new_JointDOFsDoubleArray__SWIG_0(resc,resv,argc,argv);
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addAdditionalFrameToLink" "', argument " "3"" of type '" "std::string const &""'"); 
     }
+    arg3 = ptr;
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_JointDOFsDoubleArray'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::JointDOFsDoubleArray::JointDOFsDoubleArray(unsigned int)\n"
-    "    iDynTree::JointDOFsDoubleArray::JointDOFsDoubleArray()\n"
-    "    iDynTree::JointDOFsDoubleArray::JointDOFsDoubleArray(iDynTree::Model const &)\n");
+  {
+    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
+    if (!SWIG_IsOK(res4)) {
+      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "Model_addAdditionalFrameToLink" "', argument " "4"" of type '" "iDynTree::Transform""'"); 
+    }  
+    if (!argp4) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_addAdditionalFrameToLink" "', argument " "4"" of type '" "iDynTree::Transform""'");
+    } else {
+      arg4 = *(reinterpret_cast< iDynTree::Transform * >(argp4));
+    }
+  }
+  result = (bool)(arg1)->addAdditionalFrameToLink((std::string const &)*arg2,(std::string const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_JointDOFsDoubleArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
-  unsigned int arg2 ;
+int _wrap_Model_getFrameName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
+  int val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
+  std::string result;
   
-  if (!SWIG_check_num_args("JointDOFsDoubleArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_getFrameName",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointDOFsDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameName" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "JointDOFsDoubleArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getFrameName" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
   } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = ((iDynTree::Model const *)arg1)->getFrameName(arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56024,106 +55821,69 @@ int _wrap_JointDOFsDoubleArray_resize__SWIG_0(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_JointDOFsDoubleArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_Model_getFrameIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
+  iDynTree::FrameIndex result;
   
-  if (!SWIG_check_num_args("JointDOFsDoubleArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_getFrameIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointDOFsDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointDOFsDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointDOFsDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getFrameIndex" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getFrameIndex" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  result = (iDynTree::FrameIndex)((iDynTree::Model const *)arg1)->getFrameIndex((std::string const &)*arg2);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_JointDOFsDoubleArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_JointDOFsDoubleArray_resize__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_JointDOFsDoubleArray_resize__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'JointDOFsDoubleArray_resize'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::JointDOFsDoubleArray::resize(unsigned int)\n"
-    "    iDynTree::JointDOFsDoubleArray::resize(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_JointDOFsDoubleArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_Model_isValidFrameIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("JointDOFsDoubleArray_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_isValidFrameIndex",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointDOFsDoubleArray_isConsistent" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointDOFsDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointDOFsDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_isValidFrameIndex" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::JointDOFsDoubleArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_isValidFrameIndex" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = (bool)((iDynTree::Model const *)arg1)->isValidFrameIndex(arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -56132,26 +55892,31 @@ int _wrap_JointDOFsDoubleArray_isConsistent(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_delete_JointDOFsDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
+int _wrap_Model_getFrameTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::Transform result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_JointDOFsDoubleArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getFrameTransform",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_JointDOFsDoubleArray" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameTransform" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getFrameTransform" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = ((iDynTree::Model const *)arg1)->getFrameTransform(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56159,23 +55924,31 @@ int _wrap_delete_JointDOFsDoubleArray(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_DOFSpatialForceArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
+int _wrap_Model_getFrameLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::DOFSpatialForceArray *result = 0 ;
+  iDynTree::LinkIndex result;
   
-  if (!SWIG_check_num_args("new_DOFSpatialForceArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getFrameLink",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_DOFSpatialForceArray" "', argument " "1"" of type '" "unsigned int""'");
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getFrameLink" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getFrameLink" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
   } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::DOFSpatialForceArray *)new iDynTree::DOFSpatialForceArray(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 1 |  0 );
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = (iDynTree::LinkIndex)((iDynTree::Model const *)arg1)->getFrameLink(arg2);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56183,16 +55956,31 @@ int _wrap_new_DOFSpatialForceArray__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_DOFSpatialForceArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_getNrOfNeighbors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::DOFSpatialForceArray *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("new_DOFSpatialForceArray",argc,0,0,0)) {
+  if (!SWIG_check_num_args("Model_getNrOfNeighbors",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::DOFSpatialForceArray *)new iDynTree::DOFSpatialForceArray();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNrOfNeighbors" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getNrOfNeighbors" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (unsigned int)((iDynTree::Model const *)arg1)->getNrOfNeighbors(arg2);
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56200,26 +55988,39 @@ int _wrap_new_DOFSpatialForceArray__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_DOFSpatialForceArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_Model_getNeighbor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  unsigned int arg3 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  unsigned int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::DOFSpatialForceArray *result = 0 ;
+  iDynTree::Neighbor result;
   
-  if (!SWIG_check_num_args("new_DOFSpatialForceArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_getNeighbor",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DOFSpatialForceArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_DOFSpatialForceArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getNeighbor" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::DOFSpatialForceArray *)new iDynTree::DOFSpatialForceArray((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 1 |  0 );
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getNeighbor" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_unsigned_SS_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Model_getNeighbor" "', argument " "3"" of type '" "unsigned int""'");
+  } 
+  arg3 = static_cast< unsigned int >(val3);
+  result = ((iDynTree::Model const *)arg1)->getNeighbor(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::Neighbor(static_cast< const iDynTree::Neighbor& >(result))), SWIGTYPE_p_iDynTree__Neighbor, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56227,63 +56028,90 @@ int _wrap_new_DOFSpatialForceArray__SWIG_2(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_DOFSpatialForceArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_DOFSpatialForceArray__SWIG_1(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_DOFSpatialForceArray__SWIG_2(resc,resv,argc,argv);
-    }
+int _wrap_Model_setDefaultBaseLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("Model_setDefaultBaseLink",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_DOFSpatialForceArray__SWIG_0(resc,resv,argc,argv);
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_setDefaultBaseLink" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_setDefaultBaseLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (bool)(arg1)->setDefaultBaseLink(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_Model_getDefaultBaseLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkIndex result;
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_DOFSpatialForceArray'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::DOFSpatialForceArray::DOFSpatialForceArray(unsigned int)\n"
-    "    iDynTree::DOFSpatialForceArray::DOFSpatialForceArray()\n"
-    "    iDynTree::DOFSpatialForceArray::DOFSpatialForceArray(iDynTree::Model const &)\n");
+  if (!SWIG_check_num_args("Model_getDefaultBaseLink",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getDefaultBaseLink" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::Model const *)arg1)->getDefaultBaseLink();
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_DOFSpatialForceArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
-  unsigned int arg2 ;
+int _wrap_Model_computeFullTreeTraversal__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("DOFSpatialForceArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_computeFullTreeTraversal",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_computeFullTreeTraversal" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialForceArray_resize" "', argument " "2"" of type '" "unsigned int""'");
-  } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  result = (bool)((iDynTree::Model const *)arg1)->computeFullTreeTraversal(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56291,33 +56119,42 @@ int _wrap_DOFSpatialForceArray_resize__SWIG_0(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_DOFSpatialForceArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_Model_computeFullTreeTraversal__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("DOFSpatialForceArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_computeFullTreeTraversal",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_computeFullTreeTraversal" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialForceArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialForceArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_computeFullTreeTraversal" "', argument " "2"" of type '" "iDynTree::Traversal &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Model_computeFullTreeTraversal" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = (bool)((iDynTree::Model const *)arg1)->computeFullTreeTraversal(*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56325,48 +56162,88 @@ int _wrap_DOFSpatialForceArray_resize__SWIG_1(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_DOFSpatialForceArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_computeFullTreeTraversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_DOFSpatialForceArray_resize__SWIG_1(resc,resv,argc,argv);
+        return _wrap_Model_computeFullTreeTraversal__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 2) {
+  if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+      _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_DOFSpatialForceArray_resize__SWIG_0(resc,resv,argc,argv);
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_Model_computeFullTreeTraversal__SWIG_1(resc,resv,argc,argv);
+        }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialForceArray_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_computeFullTreeTraversal'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::DOFSpatialForceArray::resize(unsigned int const)\n"
-    "    iDynTree::DOFSpatialForceArray::resize(iDynTree::Model const &)\n");
+    "    iDynTree::Model::computeFullTreeTraversal(iDynTree::Traversal &) const\n"
+    "    iDynTree::Model::computeFullTreeTraversal(iDynTree::Traversal &,iDynTree::LinkIndex const) const\n");
   return 1;
 }
 
 
-int _wrap_DOFSpatialForceArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_Model_getInertialParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("Model_getInertialParameters",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getInertialParameters" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_getInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  result = (bool)((iDynTree::Model const *)arg1)->getInertialParameters(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_Model_updateInertialParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -56374,23 +56251,23 @@ int _wrap_DOFSpatialForceArray_isConsistent(int resc, mxArray *resv[], int argc,
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("DOFSpatialForceArray_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_updateInertialParameters",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_isConsistent" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_updateInertialParameters" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialForceArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "Model_updateInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialForceArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_updateInertialParameters" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::DOFSpatialForceArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  result = (bool)(arg1)->updateInertialParameters((iDynTree::VectorDynSize const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -56399,31 +56276,23 @@ int _wrap_DOFSpatialForceArray_isConsistent(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_DOFSpatialForceArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
-  size_t arg2 ;
+int _wrap_Model_visualSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialForceVector *result = 0 ;
+  iDynTree::ModelSolidShapes *result = 0 ;
   
-  if (!SWIG_check_num_args("DOFSpatialForceArray_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_visualSolidShapes",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_visualSolidShapes" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialForceArray_paren" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (iDynTree::SpatialForceVector *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialForceVector, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::ModelSolidShapes *) &(arg1)->visualSolidShapes();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56431,31 +56300,23 @@ int _wrap_DOFSpatialForceArray_paren__SWIG_0(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_DOFSpatialForceArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
-  size_t arg2 ;
+int _wrap_Model_visualSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialForceVector *result = 0 ;
+  iDynTree::ModelSolidShapes *result = 0 ;
   
-  if (!SWIG_check_num_args("DOFSpatialForceArray_paren",argc,2,2,0)) {
+  if (!SWIG_check_num_args("Model_visualSolidShapes",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_visualSolidShapes" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialForceArray_paren" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (iDynTree::SpatialForceVector *) &((iDynTree::DOFSpatialForceArray const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialForceVector, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::ModelSolidShapes *) &((iDynTree::Model const *)arg1)->visualSolidShapes();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56463,66 +56324,75 @@ int _wrap_DOFSpatialForceArray_paren__SWIG_1(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_DOFSpatialForceArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_Model_visualSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_DOFSpatialForceArray_paren__SWIG_0(resc,resv,argc,argv);
-      }
+      return _wrap_Model_visualSolidShapes__SWIG_0(resc,resv,argc,argv);
     }
   }
-  if (argc == 2) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_DOFSpatialForceArray_paren__SWIG_1(resc,resv,argc,argv);
-      }
+      return _wrap_Model_visualSolidShapes__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialForceArray_paren'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_visualSolidShapes'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::DOFSpatialForceArray::operator ()(size_t const)\n"
-    "    iDynTree::DOFSpatialForceArray::operator ()(size_t const) const\n");
+    "    iDynTree::Model::visualSolidShapes()\n"
+    "    iDynTree::Model::visualSolidShapes() const\n");
   return 1;
 }
 
 
-int _wrap_delete_DOFSpatialForceArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
+int _wrap_Model_collisionSolidShapes__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::ModelSolidShapes *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_DOFSpatialForceArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("Model_collisionSolidShapes",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_DOFSpatialForceArray" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_collisionSolidShapes" "', argument " "1"" of type '" "iDynTree::Model *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::ModelSolidShapes *) &(arg1)->collisionSolidShapes();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_Model_collisionSolidShapes__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::ModelSolidShapes *result = 0 ;
+  
+  if (!SWIG_check_num_args("Model_collisionSolidShapes",argc,1,1,0)) {
+    SWIG_fail;
   }
-  _out = (mxArray*)0;
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_collisionSolidShapes" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::ModelSolidShapes *) &((iDynTree::Model const *)arg1)->collisionSolidShapes();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelSolidShapes, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56530,23 +56400,75 @@ int _wrap_delete_DOFSpatialForceArray(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_DOFSpatialMotionArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_Model_collisionSolidShapes(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_Model_collisionSolidShapes__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_Model_collisionSolidShapes__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'Model_collisionSolidShapes'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::Model::collisionSolidShapes()\n"
+    "    iDynTree::Model::collisionSolidShapes() const\n");
+  return 1;
+}
+
+
+int _wrap_Model_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::string result;
+  
+  if (!SWIG_check_num_args("Model_toString",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_toString" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = ((iDynTree::Model const *)arg1)->toString();
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_JointPosDoubleArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   unsigned int arg1 ;
   unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::DOFSpatialMotionArray *result = 0 ;
+  iDynTree::JointPosDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_DOFSpatialMotionArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_JointPosDoubleArray",argc,1,1,0)) {
     SWIG_fail;
   }
   ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_DOFSpatialMotionArray" "', argument " "1"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_JointPosDoubleArray" "', argument " "1"" of type '" "unsigned int""'");
   } 
   arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::DOFSpatialMotionArray *)new iDynTree::DOFSpatialMotionArray(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 1 |  0 );
+  result = (iDynTree::JointPosDoubleArray *)new iDynTree::JointPosDoubleArray(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56554,16 +56476,16 @@ int _wrap_new_DOFSpatialMotionArray__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_DOFSpatialMotionArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_JointPosDoubleArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::DOFSpatialMotionArray *result = 0 ;
+  iDynTree::JointPosDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_DOFSpatialMotionArray",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_JointPosDoubleArray",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::DOFSpatialMotionArray *)new iDynTree::DOFSpatialMotionArray();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 1 |  0 );
+  result = (iDynTree::JointPosDoubleArray *)new iDynTree::JointPosDoubleArray();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56571,26 +56493,26 @@ int _wrap_new_DOFSpatialMotionArray__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_DOFSpatialMotionArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_JointPosDoubleArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::DOFSpatialMotionArray *result = 0 ;
+  iDynTree::JointPosDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_DOFSpatialMotionArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_JointPosDoubleArray",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DOFSpatialMotionArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_JointPosDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_DOFSpatialMotionArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_JointPosDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::DOFSpatialMotionArray *)new iDynTree::DOFSpatialMotionArray((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 1 |  0 );
+  result = (iDynTree::JointPosDoubleArray *)new iDynTree::JointPosDoubleArray((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56598,9 +56520,9 @@ int _wrap_new_DOFSpatialMotionArray__SWIG_2(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_JointPosDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_DOFSpatialMotionArray__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_JointPosDoubleArray__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -56608,7 +56530,7 @@ int _wrap_new_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxArray
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_DOFSpatialMotionArray__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_JointPosDoubleArray__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
@@ -56618,21 +56540,21 @@ int _wrap_new_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxArray
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_DOFSpatialMotionArray__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_JointPosDoubleArray__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_DOFSpatialMotionArray'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_JointPosDoubleArray'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::DOFSpatialMotionArray::DOFSpatialMotionArray(unsigned int)\n"
-    "    iDynTree::DOFSpatialMotionArray::DOFSpatialMotionArray()\n"
-    "    iDynTree::DOFSpatialMotionArray::DOFSpatialMotionArray(iDynTree::Model const &)\n");
+    "    iDynTree::JointPosDoubleArray::JointPosDoubleArray(unsigned int)\n"
+    "    iDynTree::JointPosDoubleArray::JointPosDoubleArray()\n"
+    "    iDynTree::JointPosDoubleArray::JointPosDoubleArray(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_DOFSpatialMotionArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+int _wrap_JointPosDoubleArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
   unsigned int arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -56640,17 +56562,17 @@ int _wrap_DOFSpatialMotionArray_resize__SWIG_0(int resc, mxArray *resv[], int ar
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("DOFSpatialMotionArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("JointPosDoubleArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointPosDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
   ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialMotionArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "JointPosDoubleArray_resize" "', argument " "2"" of type '" "unsigned int""'");
   } 
   arg2 = static_cast< unsigned int >(val2);
   (arg1)->resize(arg2);
@@ -56662,8 +56584,8 @@ int _wrap_DOFSpatialMotionArray_resize__SWIG_0(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_DOFSpatialMotionArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+int _wrap_JointPosDoubleArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -56671,20 +56593,20 @@ int _wrap_DOFSpatialMotionArray_resize__SWIG_1(int resc, mxArray *resv[], int ar
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("DOFSpatialMotionArray_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("JointPosDoubleArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointPosDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialMotionArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointPosDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialMotionArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointPosDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -56696,25 +56618,25 @@ int _wrap_DOFSpatialMotionArray_resize__SWIG_1(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_DOFSpatialMotionArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_JointPosDoubleArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
       int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_DOFSpatialMotionArray_resize__SWIG_1(resc,resv,argc,argv);
+        return _wrap_JointPosDoubleArray_resize__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       {
@@ -56722,21 +56644,21 @@ int _wrap_DOFSpatialMotionArray_resize(int resc, mxArray *resv[], int argc, mxAr
         _v = SWIG_CheckState(res);
       }
       if (_v) {
-        return _wrap_DOFSpatialMotionArray_resize__SWIG_0(resc,resv,argc,argv);
+        return _wrap_JointPosDoubleArray_resize__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialMotionArray_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'JointPosDoubleArray_resize'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::DOFSpatialMotionArray::resize(unsigned int const)\n"
-    "    iDynTree::DOFSpatialMotionArray::resize(iDynTree::Model const &)\n");
+    "    iDynTree::JointPosDoubleArray::resize(unsigned int)\n"
+    "    iDynTree::JointPosDoubleArray::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_DOFSpatialMotionArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+int _wrap_JointPosDoubleArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -56745,23 +56667,23 @@ int _wrap_DOFSpatialMotionArray_isConsistent(int resc, mxArray *resv[], int argc
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("DOFSpatialMotionArray_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("JointPosDoubleArray_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_isConsistent" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointPosDoubleArray_isConsistent" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialMotionArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointPosDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialMotionArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointPosDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::DOFSpatialMotionArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  result = (bool)((iDynTree::JointPosDoubleArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -56770,126 +56692,22 @@ int _wrap_DOFSpatialMotionArray_isConsistent(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_DOFSpatialMotionArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
-  size_t arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  iDynTree::SpatialMotionVector *result = 0 ;
-  
-  if (!SWIG_check_num_args("DOFSpatialMotionArray_paren",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialMotionArray_paren" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (iDynTree::SpatialMotionVector *) &(arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialMotionVector, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_DOFSpatialMotionArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
-  size_t arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  iDynTree::SpatialMotionVector *result = 0 ;
-  
-  if (!SWIG_check_num_args("DOFSpatialMotionArray_paren",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialMotionArray_paren" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (iDynTree::SpatialMotionVector *) &((iDynTree::DOFSpatialMotionArray const *)arg1)->operator ()(arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialMotionVector, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_DOFSpatialMotionArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_DOFSpatialMotionArray_paren__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_DOFSpatialMotionArray_paren__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialMotionArray_paren'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::DOFSpatialMotionArray::operator ()(size_t const)\n"
-    "    iDynTree::DOFSpatialMotionArray::operator ()(size_t const) const\n");
-  return 1;
-}
-
-
-int _wrap_delete_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+int _wrap_delete_JointPosDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointPosDoubleArray *arg1 = (iDynTree::JointPosDoubleArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_DOFSpatialMotionArray",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_JointPosDoubleArray",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointPosDoubleArray, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_DOFSpatialMotionArray" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_JointPosDoubleArray" "', argument " "1"" of type '" "iDynTree::JointPosDoubleArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -56901,23 +56719,23 @@ int _wrap_delete_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_new_FrameFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  size_t arg1 ;
-  size_t val1 ;
+int _wrap_new_JointDOFsDoubleArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::FrameFreeFloatingJacobian *result = 0 ;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FrameFreeFloatingJacobian",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_JointDOFsDoubleArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_JointDOFsDoubleArray" "', argument " "1"" of type '" "unsigned int""'");
   } 
-  arg1 = static_cast< size_t >(val1);
-  result = (iDynTree::FrameFreeFloatingJacobian *)new iDynTree::FrameFreeFloatingJacobian(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 1 |  0 );
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::JointDOFsDoubleArray *)new iDynTree::JointDOFsDoubleArray(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56925,16 +56743,16 @@ int _wrap_new_FrameFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_FrameFreeFloatingJacobian__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_JointDOFsDoubleArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::FrameFreeFloatingJacobian *result = 0 ;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FrameFreeFloatingJacobian",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_JointDOFsDoubleArray",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::FrameFreeFloatingJacobian *)new iDynTree::FrameFreeFloatingJacobian();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 1 |  0 );
+  result = (iDynTree::JointDOFsDoubleArray *)new iDynTree::JointDOFsDoubleArray();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56942,26 +56760,26 @@ int _wrap_new_FrameFreeFloatingJacobian__SWIG_1(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_FrameFreeFloatingJacobian__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_JointDOFsDoubleArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FrameFreeFloatingJacobian *result = 0 ;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FrameFreeFloatingJacobian",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_JointDOFsDoubleArray",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_JointDOFsDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_JointDOFsDoubleArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::FrameFreeFloatingJacobian *)new iDynTree::FrameFreeFloatingJacobian((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 1 |  0 );
+  result = (iDynTree::JointDOFsDoubleArray *)new iDynTree::JointDOFsDoubleArray((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -56969,9 +56787,9 @@ int _wrap_new_FrameFreeFloatingJacobian__SWIG_2(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_FrameFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_JointDOFsDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_FrameFreeFloatingJacobian__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_JointDOFsDoubleArray__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -56979,31 +56797,62 @@ int _wrap_new_FrameFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxA
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_FrameFreeFloatingJacobian__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_JointDOFsDoubleArray__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     {
-      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_FrameFreeFloatingJacobian__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_JointDOFsDoubleArray__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FrameFreeFloatingJacobian'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_JointDOFsDoubleArray'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FrameFreeFloatingJacobian::FrameFreeFloatingJacobian(size_t)\n"
-    "    iDynTree::FrameFreeFloatingJacobian::FrameFreeFloatingJacobian()\n"
-    "    iDynTree::FrameFreeFloatingJacobian::FrameFreeFloatingJacobian(iDynTree::Model const &)\n");
+    "    iDynTree::JointDOFsDoubleArray::JointDOFsDoubleArray(unsigned int)\n"
+    "    iDynTree::JointDOFsDoubleArray::JointDOFsDoubleArray()\n"
+    "    iDynTree::JointDOFsDoubleArray::JointDOFsDoubleArray(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FrameFreeFloatingJacobian_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FrameFreeFloatingJacobian *arg1 = (iDynTree::FrameFreeFloatingJacobian *) 0 ;
+int _wrap_JointDOFsDoubleArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("JointDOFsDoubleArray_resize",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointDOFsDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "JointDOFsDoubleArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_JointDOFsDoubleArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -57011,20 +56860,20 @@ int _wrap_FrameFreeFloatingJacobian_resize(int resc, mxArray *resv[], int argc,
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FrameFreeFloatingJacobian_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("JointDOFsDoubleArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FrameFreeFloatingJacobian_resize" "', argument " "1"" of type '" "iDynTree::FrameFreeFloatingJacobian *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointDOFsDoubleArray_resize" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FrameFreeFloatingJacobian * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FrameFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointDOFsDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FrameFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointDOFsDoubleArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -57036,8 +56885,47 @@ int _wrap_FrameFreeFloatingJacobian_resize(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FrameFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FrameFreeFloatingJacobian *arg1 = (iDynTree::FrameFreeFloatingJacobian *) 0 ;
+int _wrap_JointDOFsDoubleArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_JointDOFsDoubleArray_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_JointDOFsDoubleArray_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'JointDOFsDoubleArray_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::JointDOFsDoubleArray::resize(unsigned int)\n"
+    "    iDynTree::JointDOFsDoubleArray::resize(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_JointDOFsDoubleArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -57046,23 +56934,23 @@ int _wrap_FrameFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], int
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("FrameFreeFloatingJacobian_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("JointDOFsDoubleArray_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FrameFreeFloatingJacobian_isConsistent" "', argument " "1"" of type '" "iDynTree::FrameFreeFloatingJacobian const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "JointDOFsDoubleArray_isConsistent" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FrameFreeFloatingJacobian * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FrameFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "JointDOFsDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FrameFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "JointDOFsDoubleArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::FrameFreeFloatingJacobian const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  result = (bool)((iDynTree::JointDOFsDoubleArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -57071,22 +56959,22 @@ int _wrap_FrameFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_delete_FrameFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FrameFreeFloatingJacobian *arg1 = (iDynTree::FrameFreeFloatingJacobian *) 0 ;
+int _wrap_delete_JointDOFsDoubleArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::JointDOFsDoubleArray *arg1 = (iDynTree::JointDOFsDoubleArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_FrameFreeFloatingJacobian",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_JointDOFsDoubleArray",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::FrameFreeFloatingJacobian *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_JointDOFsDoubleArray" "', argument " "1"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FrameFreeFloatingJacobian * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -57098,23 +56986,23 @@ int _wrap_delete_FrameFreeFloatingJacobian(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_MomentumFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  size_t arg1 ;
-  size_t val1 ;
+int _wrap_new_DOFSpatialForceArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::MomentumFreeFloatingJacobian *result = 0 ;
+  iDynTree::DOFSpatialForceArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_MomentumFreeFloatingJacobian",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_DOFSpatialForceArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_DOFSpatialForceArray" "', argument " "1"" of type '" "unsigned int""'");
   } 
-  arg1 = static_cast< size_t >(val1);
-  result = (iDynTree::MomentumFreeFloatingJacobian *)new iDynTree::MomentumFreeFloatingJacobian(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 1 |  0 );
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::DOFSpatialForceArray *)new iDynTree::DOFSpatialForceArray(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57122,16 +57010,16 @@ int _wrap_new_MomentumFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_new_MomentumFreeFloatingJacobian__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DOFSpatialForceArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::MomentumFreeFloatingJacobian *result = 0 ;
+  iDynTree::DOFSpatialForceArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_MomentumFreeFloatingJacobian",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_DOFSpatialForceArray",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::MomentumFreeFloatingJacobian *)new iDynTree::MomentumFreeFloatingJacobian();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 1 |  0 );
+  result = (iDynTree::DOFSpatialForceArray *)new iDynTree::DOFSpatialForceArray();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57139,26 +57027,26 @@ int _wrap_new_MomentumFreeFloatingJacobian__SWIG_1(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_new_MomentumFreeFloatingJacobian__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DOFSpatialForceArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::MomentumFreeFloatingJacobian *result = 0 ;
+  iDynTree::DOFSpatialForceArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_MomentumFreeFloatingJacobian",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_DOFSpatialForceArray",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DOFSpatialForceArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_DOFSpatialForceArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::MomentumFreeFloatingJacobian *)new iDynTree::MomentumFreeFloatingJacobian((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 1 |  0 );
+  result = (iDynTree::DOFSpatialForceArray *)new iDynTree::DOFSpatialForceArray((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57166,9 +57054,9 @@ int _wrap_new_MomentumFreeFloatingJacobian__SWIG_2(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_new_MomentumFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DOFSpatialForceArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_MomentumFreeFloatingJacobian__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_DOFSpatialForceArray__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -57176,31 +57064,62 @@ int _wrap_new_MomentumFreeFloatingJacobian(int resc, mxArray *resv[], int argc,
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_MomentumFreeFloatingJacobian__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_DOFSpatialForceArray__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     {
-      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_MomentumFreeFloatingJacobian__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_DOFSpatialForceArray__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_MomentumFreeFloatingJacobian'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_DOFSpatialForceArray'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::MomentumFreeFloatingJacobian::MomentumFreeFloatingJacobian(size_t)\n"
-    "    iDynTree::MomentumFreeFloatingJacobian::MomentumFreeFloatingJacobian()\n"
-    "    iDynTree::MomentumFreeFloatingJacobian::MomentumFreeFloatingJacobian(iDynTree::Model const &)\n");
+    "    iDynTree::DOFSpatialForceArray::DOFSpatialForceArray(unsigned int)\n"
+    "    iDynTree::DOFSpatialForceArray::DOFSpatialForceArray()\n"
+    "    iDynTree::DOFSpatialForceArray::DOFSpatialForceArray(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_MomentumFreeFloatingJacobian_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MomentumFreeFloatingJacobian *arg1 = (iDynTree::MomentumFreeFloatingJacobian *) 0 ;
+int _wrap_DOFSpatialForceArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("DOFSpatialForceArray_resize",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialForceArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DOFSpatialForceArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -57208,20 +57127,20 @@ int _wrap_MomentumFreeFloatingJacobian_resize(int resc, mxArray *resv[], int arg
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("MomentumFreeFloatingJacobian_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DOFSpatialForceArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MomentumFreeFloatingJacobian_resize" "', argument " "1"" of type '" "iDynTree::MomentumFreeFloatingJacobian *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MomentumFreeFloatingJacobian * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MomentumFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialForceArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MomentumFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialForceArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -57233,8 +57152,47 @@ int _wrap_MomentumFreeFloatingJacobian_resize(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_MomentumFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MomentumFreeFloatingJacobian *arg1 = (iDynTree::MomentumFreeFloatingJacobian *) 0 ;
+int _wrap_DOFSpatialForceArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_DOFSpatialForceArray_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_DOFSpatialForceArray_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialForceArray_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::DOFSpatialForceArray::resize(unsigned int const)\n"
+    "    iDynTree::DOFSpatialForceArray::resize(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_DOFSpatialForceArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -57243,23 +57201,23 @@ int _wrap_MomentumFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], i
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("MomentumFreeFloatingJacobian_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DOFSpatialForceArray_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MomentumFreeFloatingJacobian_isConsistent" "', argument " "1"" of type '" "iDynTree::MomentumFreeFloatingJacobian const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_isConsistent" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MomentumFreeFloatingJacobian * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MomentumFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialForceArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MomentumFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialForceArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)((iDynTree::MomentumFreeFloatingJacobian const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  result = (bool)((iDynTree::DOFSpatialForceArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -57268,22 +57226,126 @@ int _wrap_MomentumFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_delete_MomentumFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::MomentumFreeFloatingJacobian *arg1 = (iDynTree::MomentumFreeFloatingJacobian *) 0 ;
+int _wrap_DOFSpatialForceArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::SpatialForceVector *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_MomentumFreeFloatingJacobian",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DOFSpatialForceArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::MomentumFreeFloatingJacobian *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::MomentumFreeFloatingJacobian * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialForceArray_paren" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (iDynTree::SpatialForceVector *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialForceVector, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DOFSpatialForceArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
+  size_t arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  iDynTree::SpatialForceVector *result = 0 ;
+  
+  if (!SWIG_check_num_args("DOFSpatialForceArray_paren",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialForceArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialForceArray_paren" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (iDynTree::SpatialForceVector *) &((iDynTree::DOFSpatialForceArray const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialForceVector, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DOFSpatialForceArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_DOFSpatialForceArray_paren__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_DOFSpatialForceArray_paren__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialForceArray_paren'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::DOFSpatialForceArray::operator ()(size_t const)\n"
+    "    iDynTree::DOFSpatialForceArray::operator ()(size_t const) const\n");
+  return 1;
+}
+
+
+int _wrap_delete_DOFSpatialForceArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialForceArray *arg1 = (iDynTree::DOFSpatialForceArray *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_DOFSpatialForceArray",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_DOFSpatialForceArray" "', argument " "1"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -57295,23 +57357,23 @@ int _wrap_delete_MomentumFreeFloatingJacobian(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_new_FreeFloatingMassMatrix__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  size_t arg1 ;
-  size_t val1 ;
+int _wrap_new_DOFSpatialMotionArray__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
   int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingMassMatrix *result = 0 ;
+  iDynTree::DOFSpatialMotionArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingMassMatrix",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_DOFSpatialMotionArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
   if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_FreeFloatingMassMatrix" "', argument " "1"" of type '" "size_t""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_DOFSpatialMotionArray" "', argument " "1"" of type '" "unsigned int""'");
   } 
-  arg1 = static_cast< size_t >(val1);
-  result = (iDynTree::FreeFloatingMassMatrix *)new iDynTree::FreeFloatingMassMatrix(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 1 |  0 );
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::DOFSpatialMotionArray *)new iDynTree::DOFSpatialMotionArray(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57319,16 +57381,16 @@ int _wrap_new_FreeFloatingMassMatrix__SWIG_0(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_new_FreeFloatingMassMatrix__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DOFSpatialMotionArray__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::FreeFloatingMassMatrix *result = 0 ;
+  iDynTree::DOFSpatialMotionArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingMassMatrix",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_DOFSpatialMotionArray",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::FreeFloatingMassMatrix *)new iDynTree::FreeFloatingMassMatrix();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 1 |  0 );
+  result = (iDynTree::DOFSpatialMotionArray *)new iDynTree::DOFSpatialMotionArray();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57336,26 +57398,26 @@ int _wrap_new_FreeFloatingMassMatrix__SWIG_1(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_new_FreeFloatingMassMatrix__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DOFSpatialMotionArray__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingMassMatrix *result = 0 ;
+  iDynTree::DOFSpatialMotionArray *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingMassMatrix",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_DOFSpatialMotionArray",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingMassMatrix" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_DOFSpatialMotionArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingMassMatrix" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_DOFSpatialMotionArray" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::FreeFloatingMassMatrix *)new iDynTree::FreeFloatingMassMatrix((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 1 |  0 );
+  result = (iDynTree::DOFSpatialMotionArray *)new iDynTree::DOFSpatialMotionArray((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57363,9 +57425,9 @@ int _wrap_new_FreeFloatingMassMatrix__SWIG_2(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_new_FreeFloatingMassMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_FreeFloatingMassMatrix__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_DOFSpatialMotionArray__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -57373,31 +57435,62 @@ int _wrap_new_FreeFloatingMassMatrix(int resc, mxArray *resv[], int argc, mxArra
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_FreeFloatingMassMatrix__SWIG_2(resc,resv,argc,argv);
+      return _wrap_new_DOFSpatialMotionArray__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     {
-      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
       _v = SWIG_CheckState(res);
     }
     if (_v) {
-      return _wrap_new_FreeFloatingMassMatrix__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_DOFSpatialMotionArray__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingMassMatrix'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_DOFSpatialMotionArray'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingMassMatrix::FreeFloatingMassMatrix(size_t)\n"
-    "    iDynTree::FreeFloatingMassMatrix::FreeFloatingMassMatrix()\n"
-    "    iDynTree::FreeFloatingMassMatrix::FreeFloatingMassMatrix(iDynTree::Model const &)\n");
+    "    iDynTree::DOFSpatialMotionArray::DOFSpatialMotionArray(unsigned int)\n"
+    "    iDynTree::DOFSpatialMotionArray::DOFSpatialMotionArray()\n"
+    "    iDynTree::DOFSpatialMotionArray::DOFSpatialMotionArray(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingMassMatrix_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingMassMatrix *arg1 = (iDynTree::FreeFloatingMassMatrix *) 0 ;
+int _wrap_DOFSpatialMotionArray_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("DOFSpatialMotionArray_resize",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialMotionArray_resize" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DOFSpatialMotionArray_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -57405,20 +57498,20 @@ int _wrap_FreeFloatingMassMatrix_resize(int resc, mxArray *resv[], int argc, mxA
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FreeFloatingMassMatrix_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DOFSpatialMotionArray_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingMassMatrix_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingMassMatrix *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_resize" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingMassMatrix * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingMassMatrix_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialMotionArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingMassMatrix_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialMotionArray_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -57430,26 +57523,73 @@ int _wrap_FreeFloatingMassMatrix_resize(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_delete_FreeFloatingMassMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingMassMatrix *arg1 = (iDynTree::FreeFloatingMassMatrix *) 0 ;
+int _wrap_DOFSpatialMotionArray_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_DOFSpatialMotionArray_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_DOFSpatialMotionArray_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialMotionArray_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::DOFSpatialMotionArray::resize(unsigned int const)\n"
+    "    iDynTree::DOFSpatialMotionArray::resize(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_DOFSpatialMotionArray_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_FreeFloatingMassMatrix",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DOFSpatialMotionArray_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingMassMatrix" "', argument " "1"" of type '" "iDynTree::FreeFloatingMassMatrix *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_isConsistent" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingMassMatrix * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DOFSpatialMotionArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DOFSpatialMotionArray_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::DOFSpatialMotionArray const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57457,16 +57597,31 @@ int _wrap_delete_FreeFloatingMassMatrix(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_new_FreeFloatingPos__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DOFSpatialMotionArray_paren__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+  size_t arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingPos *result = 0 ;
+  iDynTree::SpatialMotionVector *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingPos",argc,0,0,0)) {
+  if (!SWIG_check_num_args("DOFSpatialMotionArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::FreeFloatingPos *)new iDynTree::FreeFloatingPos();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingPos, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialMotionArray_paren" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (iDynTree::SpatialMotionVector *) &(arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialMotionVector, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57474,26 +57629,31 @@ int _wrap_new_FreeFloatingPos__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_FreeFloatingPos__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_DOFSpatialMotionArray_paren__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
+  size_t arg2 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingPos *result = 0 ;
+  iDynTree::SpatialMotionVector *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingPos",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DOFSpatialMotionArray_paren",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingPos" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingPos" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DOFSpatialMotionArray_paren" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::FreeFloatingPos *)new iDynTree::FreeFloatingPos((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingPos, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DOFSpatialMotionArray_paren" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (iDynTree::SpatialMotionVector *) &((iDynTree::DOFSpatialMotionArray const *)arg1)->operator ()(arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialMotionVector, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57501,54 +57661,65 @@ int _wrap_new_FreeFloatingPos__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_FreeFloatingPos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_FreeFloatingPos__SWIG_0(resc,resv,argc,argv);
+int _wrap_DOFSpatialMotionArray_paren(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_DOFSpatialMotionArray_paren__SWIG_0(resc,resv,argc,argv);
+      }
+    }
   }
-  if (argc == 1) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_FreeFloatingPos__SWIG_1(resc,resv,argc,argv);
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_DOFSpatialMotionArray_paren__SWIG_1(resc,resv,argc,argv);
+      }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingPos'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DOFSpatialMotionArray_paren'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingPos::FreeFloatingPos()\n"
-    "    iDynTree::FreeFloatingPos::FreeFloatingPos(iDynTree::Model const &)\n");
+    "    iDynTree::DOFSpatialMotionArray::operator ()(size_t const)\n"
+    "    iDynTree::DOFSpatialMotionArray::operator ()(size_t const) const\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingPos_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_delete_DOFSpatialMotionArray(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DOFSpatialMotionArray *arg1 = (iDynTree::DOFSpatialMotionArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FreeFloatingPos_resize",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_DOFSpatialMotionArray",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingPos_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_DOFSpatialMotionArray" "', argument " "1"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingPos_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -57557,23 +57728,23 @@ int _wrap_FreeFloatingPos_resize(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_FreeFloatingPos_worldBasePos__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_FrameFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  size_t arg1 ;
+  size_t val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
+  iDynTree::FrameFreeFloatingJacobian *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingPos_worldBasePos",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FrameFreeFloatingJacobian",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_worldBasePos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
-  result = (iDynTree::Transform *) &(arg1)->worldBasePos();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "size_t""'");
+  } 
+  arg1 = static_cast< size_t >(val1);
+  result = (iDynTree::FrameFreeFloatingJacobian *)new iDynTree::FrameFreeFloatingJacobian(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57581,23 +57752,16 @@ int _wrap_FreeFloatingPos_worldBasePos__SWIG_0(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_FreeFloatingPos_jointPos__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_FrameFreeFloatingJacobian__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::JointPosDoubleArray *result = 0 ;
+  iDynTree::FrameFreeFloatingJacobian *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingPos_jointPos",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FrameFreeFloatingJacobian",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_jointPos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
-  result = (iDynTree::JointPosDoubleArray *) &(arg1)->jointPos();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
+  (void)argv;
+  result = (iDynTree::FrameFreeFloatingJacobian *)new iDynTree::FrameFreeFloatingJacobian();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57605,23 +57769,26 @@ int _wrap_FreeFloatingPos_jointPos__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FreeFloatingPos_worldBasePos__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_FrameFreeFloatingJacobian__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Transform *result = 0 ;
+  iDynTree::FrameFreeFloatingJacobian *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingPos_worldBasePos",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FrameFreeFloatingJacobian",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_worldBasePos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
-  result = (iDynTree::Transform *) &((iDynTree::FreeFloatingPos const *)arg1)->worldBasePos();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::FrameFreeFloatingJacobian *)new iDynTree::FrameFreeFloatingJacobian((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57629,51 +57796,66 @@ int _wrap_FreeFloatingPos_worldBasePos__SWIG_1(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_FreeFloatingPos_worldBasePos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FrameFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_FrameFreeFloatingJacobian__SWIG_1(resc,resv,argc,argv);
+  }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingPos_worldBasePos__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_FrameFreeFloatingJacobian__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
-    _v = SWIG_CheckState(res);
+    {
+      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
     if (_v) {
-      return _wrap_FreeFloatingPos_worldBasePos__SWIG_1(resc,resv,argc,argv);
+      return _wrap_new_FrameFreeFloatingJacobian__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingPos_worldBasePos'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FrameFreeFloatingJacobian'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingPos::worldBasePos()\n"
-    "    iDynTree::FreeFloatingPos::worldBasePos() const\n");
+    "    iDynTree::FrameFreeFloatingJacobian::FrameFreeFloatingJacobian(size_t)\n"
+    "    iDynTree::FrameFreeFloatingJacobian::FrameFreeFloatingJacobian()\n"
+    "    iDynTree::FrameFreeFloatingJacobian::FrameFreeFloatingJacobian(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingPos_jointPos__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
+int _wrap_FrameFreeFloatingJacobian_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FrameFreeFloatingJacobian *arg1 = (iDynTree::FrameFreeFloatingJacobian *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::JointPosDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingPos_jointPos",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FrameFreeFloatingJacobian_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_jointPos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FrameFreeFloatingJacobian_resize" "', argument " "1"" of type '" "iDynTree::FrameFreeFloatingJacobian *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
-  result = (iDynTree::JointPosDoubleArray *) &((iDynTree::FreeFloatingPos const *)arg1)->jointPos();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FrameFreeFloatingJacobian * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FrameFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FrameFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57681,51 +57863,34 @@ int _wrap_FreeFloatingPos_jointPos__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FreeFloatingPos_jointPos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_FreeFloatingPos_jointPos__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_FreeFloatingPos_jointPos__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingPos_jointPos'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingPos::jointPos()\n"
-    "    iDynTree::FreeFloatingPos::jointPos() const\n");
-  return 1;
-}
-
-
-int _wrap_FreeFloatingPos_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
+int _wrap_FrameFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FrameFreeFloatingJacobian *arg1 = (iDynTree::FrameFreeFloatingJacobian *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  unsigned int result;
+  bool result;
   
-  if (!SWIG_check_num_args("FreeFloatingPos_getNrOfPosCoords",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FrameFreeFloatingJacobian_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FrameFreeFloatingJacobian_isConsistent" "', argument " "1"" of type '" "iDynTree::FrameFreeFloatingJacobian const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
-  result = (unsigned int)((iDynTree::FreeFloatingPos const *)arg1)->getNrOfPosCoords();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FrameFreeFloatingJacobian * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FrameFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FrameFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::FrameFreeFloatingJacobian const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57733,22 +57898,22 @@ int _wrap_FreeFloatingPos_getNrOfPosCoords(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_delete_FreeFloatingPos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
+int _wrap_delete_FrameFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FrameFreeFloatingJacobian *arg1 = (iDynTree::FrameFreeFloatingJacobian *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_FreeFloatingPos",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_FrameFreeFloatingJacobian",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FrameFreeFloatingJacobian, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingPos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FrameFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::FrameFreeFloatingJacobian *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FrameFreeFloatingJacobian * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -57760,16 +57925,40 @@ int _wrap_delete_FreeFloatingPos(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_new_FreeFloatingGeneralizedTorques__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_MomentumFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  size_t arg1 ;
+  size_t val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingGeneralizedTorques *result = 0 ;
+  iDynTree::MomentumFreeFloatingJacobian *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingGeneralizedTorques",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_MomentumFreeFloatingJacobian",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "size_t""'");
+  } 
+  arg1 = static_cast< size_t >(val1);
+  result = (iDynTree::MomentumFreeFloatingJacobian *)new iDynTree::MomentumFreeFloatingJacobian(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_MomentumFreeFloatingJacobian__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::MomentumFreeFloatingJacobian *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_MomentumFreeFloatingJacobian",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::FreeFloatingGeneralizedTorques *)new iDynTree::FreeFloatingGeneralizedTorques();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 1 |  0 );
+  result = (iDynTree::MomentumFreeFloatingJacobian *)new iDynTree::MomentumFreeFloatingJacobian();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57777,26 +57966,26 @@ int _wrap_new_FreeFloatingGeneralizedTorques__SWIG_0(int resc, mxArray *resv[],
 }
 
 
-int _wrap_new_FreeFloatingGeneralizedTorques__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_MomentumFreeFloatingJacobian__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingGeneralizedTorques *result = 0 ;
+  iDynTree::MomentumFreeFloatingJacobian *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingGeneralizedTorques",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_MomentumFreeFloatingJacobian",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingGeneralizedTorques" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingGeneralizedTorques" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::FreeFloatingGeneralizedTorques *)new iDynTree::FreeFloatingGeneralizedTorques((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 1 |  0 );
+  result = (iDynTree::MomentumFreeFloatingJacobian *)new iDynTree::MomentumFreeFloatingJacobian((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57804,9 +57993,9 @@ int _wrap_new_FreeFloatingGeneralizedTorques__SWIG_1(int resc, mxArray *resv[],
 }
 
 
-int _wrap_new_FreeFloatingGeneralizedTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_MomentumFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_FreeFloatingGeneralizedTorques__SWIG_0(resc,resv,argc,argv);
+    return _wrap_new_MomentumFreeFloatingJacobian__SWIG_1(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -57814,20 +58003,31 @@ int _wrap_new_FreeFloatingGeneralizedTorques(int resc, mxArray *resv[], int argc
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_FreeFloatingGeneralizedTorques__SWIG_1(resc,resv,argc,argv);
+      return _wrap_new_MomentumFreeFloatingJacobian__SWIG_2(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    {
+      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      return _wrap_new_MomentumFreeFloatingJacobian__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingGeneralizedTorques'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_MomentumFreeFloatingJacobian'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingGeneralizedTorques::FreeFloatingGeneralizedTorques()\n"
-    "    iDynTree::FreeFloatingGeneralizedTorques::FreeFloatingGeneralizedTorques(iDynTree::Model const &)\n");
+    "    iDynTree::MomentumFreeFloatingJacobian::MomentumFreeFloatingJacobian(size_t)\n"
+    "    iDynTree::MomentumFreeFloatingJacobian::MomentumFreeFloatingJacobian()\n"
+    "    iDynTree::MomentumFreeFloatingJacobian::MomentumFreeFloatingJacobian(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
+int _wrap_MomentumFreeFloatingJacobian_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MomentumFreeFloatingJacobian *arg1 = (iDynTree::MomentumFreeFloatingJacobian *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -57835,20 +58035,20 @@ int _wrap_FreeFloatingGeneralizedTorques_resize(int resc, mxArray *resv[], int a
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("MomentumFreeFloatingJacobian_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MomentumFreeFloatingJacobian_resize" "', argument " "1"" of type '" "iDynTree::MomentumFreeFloatingJacobian *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::MomentumFreeFloatingJacobian * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingGeneralizedTorques_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MomentumFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingGeneralizedTorques_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MomentumFreeFloatingJacobian_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -57860,23 +58060,34 @@ int _wrap_FreeFloatingGeneralizedTorques_resize(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
+int _wrap_MomentumFreeFloatingJacobian_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MomentumFreeFloatingJacobian *arg1 = (iDynTree::MomentumFreeFloatingJacobian *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_baseWrench",argc,1,1,0)) {
+  if (!SWIG_check_num_args("MomentumFreeFloatingJacobian_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_baseWrench" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "MomentumFreeFloatingJacobian_isConsistent" "', argument " "1"" of type '" "iDynTree::MomentumFreeFloatingJacobian const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
-  result = (iDynTree::Wrench *) &(arg1)->baseWrench();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::MomentumFreeFloatingJacobian * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "MomentumFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "MomentumFreeFloatingJacobian_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)((iDynTree::MomentumFreeFloatingJacobian const *)arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57884,23 +58095,26 @@ int _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_0(int resc, mxArray *r
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
+int _wrap_delete_MomentumFreeFloatingJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::MomentumFreeFloatingJacobian *arg1 = (iDynTree::MomentumFreeFloatingJacobian *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_jointTorques",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_MomentumFreeFloatingJacobian",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_jointTorques" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_MomentumFreeFloatingJacobian" "', argument " "1"" of type '" "iDynTree::MomentumFreeFloatingJacobian *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *) &(arg1)->jointTorques();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::MomentumFreeFloatingJacobian * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57908,23 +58122,23 @@ int _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_0(int resc, mxArray
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_FreeFloatingMassMatrix__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  size_t arg1 ;
+  size_t val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  iDynTree::FreeFloatingMassMatrix *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_baseWrench",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingMassMatrix",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_baseWrench" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
-  result = (iDynTree::Wrench *) &((iDynTree::FreeFloatingGeneralizedTorques const *)arg1)->baseWrench();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_FreeFloatingMassMatrix" "', argument " "1"" of type '" "size_t""'");
+  } 
+  arg1 = static_cast< size_t >(val1);
+  result = (iDynTree::FreeFloatingMassMatrix *)new iDynTree::FreeFloatingMassMatrix(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57932,51 +58146,43 @@ int _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_1(int resc, mxArray *r
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_baseWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_1(resc,resv,argc,argv);
-    }
-  }
+int _wrap_new_FreeFloatingMassMatrix__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::FreeFloatingMassMatrix *result = 0 ;
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingGeneralizedTorques_baseWrench'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingGeneralizedTorques::baseWrench()\n"
-    "    iDynTree::FreeFloatingGeneralizedTorques::baseWrench() const\n");
+  if (!SWIG_check_num_args("new_FreeFloatingMassMatrix",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::FreeFloatingMassMatrix *)new iDynTree::FreeFloatingMassMatrix();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_FreeFloatingMassMatrix__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  iDynTree::FreeFloatingMassMatrix *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_jointTorques",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingMassMatrix",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_jointTorques" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingMassMatrix" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *) &((iDynTree::FreeFloatingGeneralizedTorques const *)arg1)->jointTorques();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingMassMatrix" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::FreeFloatingMassMatrix *)new iDynTree::FreeFloatingMassMatrix((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -57984,51 +58190,66 @@ int _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_1(int resc, mxArray
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_jointTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingMassMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_FreeFloatingMassMatrix__SWIG_1(resc,resv,argc,argv);
+  }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_FreeFloatingMassMatrix__SWIG_2(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
-    _v = SWIG_CheckState(res);
+    {
+      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
     if (_v) {
-      return _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_1(resc,resv,argc,argv);
+      return _wrap_new_FreeFloatingMassMatrix__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingGeneralizedTorques_jointTorques'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingMassMatrix'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingGeneralizedTorques::jointTorques()\n"
-    "    iDynTree::FreeFloatingGeneralizedTorques::jointTorques() const\n");
+    "    iDynTree::FreeFloatingMassMatrix::FreeFloatingMassMatrix(size_t)\n"
+    "    iDynTree::FreeFloatingMassMatrix::FreeFloatingMassMatrix()\n"
+    "    iDynTree::FreeFloatingMassMatrix::FreeFloatingMassMatrix(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
+int _wrap_FreeFloatingMassMatrix_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingMassMatrix *arg1 = (iDynTree::FreeFloatingMassMatrix *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  unsigned int result;
   
-  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingMassMatrix_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingMassMatrix_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingMassMatrix *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
-  result = (unsigned int)((iDynTree::FreeFloatingGeneralizedTorques const *)arg1)->getNrOfDOFs();
-  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingMassMatrix * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingMassMatrix_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingMassMatrix_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58036,22 +58257,22 @@ int _wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(int resc, mxArray *resv[],
 }
 
 
-int _wrap_delete_FreeFloatingGeneralizedTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
+int _wrap_delete_FreeFloatingMassMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingMassMatrix *arg1 = (iDynTree::FreeFloatingMassMatrix *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_FreeFloatingGeneralizedTorques",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_FreeFloatingMassMatrix",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingGeneralizedTorques" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingMassMatrix" "', argument " "1"" of type '" "iDynTree::FreeFloatingMassMatrix *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingMassMatrix * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -58063,16 +58284,16 @@ int _wrap_delete_FreeFloatingGeneralizedTorques(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_FreeFloatingVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingPos__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::FreeFloatingVel *result = 0 ;
+  iDynTree::FreeFloatingPos *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingVel",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingPos",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::FreeFloatingVel *)new iDynTree::FreeFloatingVel();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingVel, 1 |  0 );
+  result = (iDynTree::FreeFloatingPos *)new iDynTree::FreeFloatingPos();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingPos, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58080,26 +58301,26 @@ int _wrap_new_FreeFloatingVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_FreeFloatingVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingPos__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingVel *result = 0 ;
+  iDynTree::FreeFloatingPos *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingVel",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingPos",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingVel" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingPos" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingVel" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingPos" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::FreeFloatingVel *)new iDynTree::FreeFloatingVel((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingVel, 1 |  0 );
+  result = (iDynTree::FreeFloatingPos *)new iDynTree::FreeFloatingPos((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingPos, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58107,9 +58328,9 @@ int _wrap_new_FreeFloatingVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_FreeFloatingVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingPos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_FreeFloatingVel__SWIG_0(resc,resv,argc,argv);
+    return _wrap_new_FreeFloatingPos__SWIG_0(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -58117,20 +58338,20 @@ int _wrap_new_FreeFloatingVel(int resc, mxArray *resv[], int argc, mxArray *argv
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_FreeFloatingVel__SWIG_1(resc,resv,argc,argv);
+      return _wrap_new_FreeFloatingPos__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingVel'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingPos'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingVel::FreeFloatingVel()\n"
-    "    iDynTree::FreeFloatingVel::FreeFloatingVel(iDynTree::Model const &)\n");
+    "    iDynTree::FreeFloatingPos::FreeFloatingPos()\n"
+    "    iDynTree::FreeFloatingPos::FreeFloatingPos(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingVel_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_FreeFloatingPos_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -58138,20 +58359,20 @@ int _wrap_FreeFloatingVel_resize(int resc, mxArray *resv[], int argc, mxArray *a
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FreeFloatingVel_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FreeFloatingPos_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingVel_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingPos_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingVel_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingPos_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -58163,23 +58384,23 @@ int _wrap_FreeFloatingVel_resize(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_FreeFloatingVel_baseVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_FreeFloatingPos_worldBasePos__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Twist *result = 0 ;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingVel_baseVel",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingPos_worldBasePos",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_baseVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_worldBasePos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
-  result = (iDynTree::Twist *) &(arg1)->baseVel();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
+  result = (iDynTree::Transform *) &(arg1)->worldBasePos();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58187,23 +58408,23 @@ int _wrap_FreeFloatingVel_baseVel__SWIG_0(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_FreeFloatingVel_jointVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_FreeFloatingPos_jointPos__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  iDynTree::JointPosDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingVel_jointVel",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingPos_jointPos",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_jointVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_jointPos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *) &(arg1)->jointVel();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
+  result = (iDynTree::JointPosDoubleArray *) &(arg1)->jointPos();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58211,23 +58432,23 @@ int _wrap_FreeFloatingVel_jointVel__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FreeFloatingVel_baseVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_FreeFloatingPos_worldBasePos__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Twist *result = 0 ;
+  iDynTree::Transform *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingVel_baseVel",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingPos_worldBasePos",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_baseVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_worldBasePos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
-  result = (iDynTree::Twist *) &((iDynTree::FreeFloatingVel const *)arg1)->baseVel();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
+  result = (iDynTree::Transform *) &((iDynTree::FreeFloatingPos const *)arg1)->worldBasePos();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Transform, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58235,51 +58456,51 @@ int _wrap_FreeFloatingVel_baseVel__SWIG_1(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_FreeFloatingVel_baseVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingPos_worldBasePos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingVel_baseVel__SWIG_0(resc,resv,argc,argv);
+      return _wrap_FreeFloatingPos_worldBasePos__SWIG_0(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingVel_baseVel__SWIG_1(resc,resv,argc,argv);
+      return _wrap_FreeFloatingPos_worldBasePos__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingVel_baseVel'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingPos_worldBasePos'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingVel::baseVel()\n"
-    "    iDynTree::FreeFloatingVel::baseVel() const\n");
+    "    iDynTree::FreeFloatingPos::worldBasePos()\n"
+    "    iDynTree::FreeFloatingPos::worldBasePos() const\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingVel_jointVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_FreeFloatingPos_jointPos__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  iDynTree::JointPosDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingVel_jointVel",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingPos_jointPos",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_jointVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_jointPos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *) &((iDynTree::FreeFloatingVel const *)arg1)->jointVel();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
+  result = (iDynTree::JointPosDoubleArray *) &((iDynTree::FreeFloatingPos const *)arg1)->jointPos();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointPosDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58287,50 +58508,50 @@ int _wrap_FreeFloatingVel_jointVel__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FreeFloatingVel_jointVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingPos_jointPos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingVel_jointVel__SWIG_0(resc,resv,argc,argv);
+      return _wrap_FreeFloatingPos_jointPos__SWIG_0(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingVel_jointVel__SWIG_1(resc,resv,argc,argv);
+      return _wrap_FreeFloatingPos_jointPos__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingVel_jointVel'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingPos_jointPos'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingVel::jointVel()\n"
-    "    iDynTree::FreeFloatingVel::jointVel() const\n");
+    "    iDynTree::FreeFloatingPos::jointPos()\n"
+    "    iDynTree::FreeFloatingPos::jointPos() const\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingVel_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_FreeFloatingPos_getNrOfPosCoords(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("FreeFloatingVel_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingPos_getNrOfPosCoords",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingPos_getNrOfPosCoords" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
-  result = (unsigned int)((iDynTree::FreeFloatingVel const *)arg1)->getNrOfDOFs();
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
+  result = (unsigned int)((iDynTree::FreeFloatingPos const *)arg1)->getNrOfPosCoords();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -58339,22 +58560,22 @@ int _wrap_FreeFloatingVel_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_delete_FreeFloatingVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+int _wrap_delete_FreeFloatingPos(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingPos *arg1 = (iDynTree::FreeFloatingPos *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_FreeFloatingVel",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_FreeFloatingPos",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingPos, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingPos" "', argument " "1"" of type '" "iDynTree::FreeFloatingPos *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -58366,16 +58587,16 @@ int _wrap_delete_FreeFloatingVel(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_new_FreeFloatingAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingGeneralizedTorques__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::FreeFloatingAcc *result = 0 ;
+  iDynTree::FreeFloatingGeneralizedTorques *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingAcc",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingGeneralizedTorques",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::FreeFloatingAcc *)new iDynTree::FreeFloatingAcc();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingAcc, 1 |  0 );
+  result = (iDynTree::FreeFloatingGeneralizedTorques *)new iDynTree::FreeFloatingGeneralizedTorques();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58383,26 +58604,26 @@ int _wrap_new_FreeFloatingAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_FreeFloatingAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingGeneralizedTorques__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::FreeFloatingAcc *result = 0 ;
+  iDynTree::FreeFloatingGeneralizedTorques *result = 0 ;
   
-  if (!SWIG_check_num_args("new_FreeFloatingAcc",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingGeneralizedTorques",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingAcc" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingGeneralizedTorques" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingAcc" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingGeneralizedTorques" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::FreeFloatingAcc *)new iDynTree::FreeFloatingAcc((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingAcc, 1 |  0 );
+  result = (iDynTree::FreeFloatingGeneralizedTorques *)new iDynTree::FreeFloatingGeneralizedTorques((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58410,9 +58631,9 @@ int _wrap_new_FreeFloatingAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_FreeFloatingAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingGeneralizedTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_FreeFloatingAcc__SWIG_0(resc,resv,argc,argv);
+    return _wrap_new_FreeFloatingGeneralizedTorques__SWIG_0(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -58420,20 +58641,20 @@ int _wrap_new_FreeFloatingAcc(int resc, mxArray *resv[], int argc, mxArray *argv
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_FreeFloatingAcc__SWIG_1(resc,resv,argc,argv);
+      return _wrap_new_FreeFloatingGeneralizedTorques__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingAcc'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingGeneralizedTorques'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingAcc::FreeFloatingAcc()\n"
-    "    iDynTree::FreeFloatingAcc::FreeFloatingAcc(iDynTree::Model const &)\n");
+    "    iDynTree::FreeFloatingGeneralizedTorques::FreeFloatingGeneralizedTorques()\n"
+    "    iDynTree::FreeFloatingGeneralizedTorques::FreeFloatingGeneralizedTorques(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingAcc_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_FreeFloatingGeneralizedTorques_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -58441,20 +58662,20 @@ int _wrap_FreeFloatingAcc_resize(int resc, mxArray *resv[], int argc, mxArray *a
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("FreeFloatingAcc_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingAcc_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingGeneralizedTorques_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingAcc_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingGeneralizedTorques_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
   (arg1)->resize((iDynTree::Model const &)*arg2);
@@ -58466,23 +58687,23 @@ int _wrap_FreeFloatingAcc_resize(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_FreeFloatingAcc_baseAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialAcc *result = 0 ;
+  iDynTree::Wrench *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingAcc_baseAcc",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_baseWrench",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_baseAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_baseWrench" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
-  result = (iDynTree::SpatialAcc *) &(arg1)->baseAcc();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  result = (iDynTree::Wrench *) &(arg1)->baseWrench();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58490,22 +58711,22 @@ int _wrap_FreeFloatingAcc_baseAcc__SWIG_0(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_FreeFloatingAcc_jointAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingAcc_jointAcc",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_jointTorques",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_jointAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_jointTorques" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *) &(arg1)->jointAcc();
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *) &(arg1)->jointTorques();
   _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -58514,23 +58735,23 @@ int _wrap_FreeFloatingAcc_jointAcc__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FreeFloatingAcc_baseAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SpatialAcc *result = 0 ;
+  iDynTree::Wrench *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingAcc_baseAcc",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_baseWrench",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_baseAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_baseWrench" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
-  result = (iDynTree::SpatialAcc *) &((iDynTree::FreeFloatingAcc const *)arg1)->baseAcc();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  result = (iDynTree::Wrench *) &((iDynTree::FreeFloatingGeneralizedTorques const *)arg1)->baseWrench();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58538,50 +58759,50 @@ int _wrap_FreeFloatingAcc_baseAcc__SWIG_1(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_FreeFloatingAcc_baseAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingGeneralizedTorques_baseWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingAcc_baseAcc__SWIG_0(resc,resv,argc,argv);
+      return _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_0(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingAcc_baseAcc__SWIG_1(resc,resv,argc,argv);
+      return _wrap_FreeFloatingGeneralizedTorques_baseWrench__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingAcc_baseAcc'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingGeneralizedTorques_baseWrench'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingAcc::baseAcc()\n"
-    "    iDynTree::FreeFloatingAcc::baseAcc() const\n");
+    "    iDynTree::FreeFloatingGeneralizedTorques::baseWrench()\n"
+    "    iDynTree::FreeFloatingGeneralizedTorques::baseWrench() const\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingAcc_jointAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("FreeFloatingAcc_jointAcc",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_jointTorques",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_jointAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_jointTorques" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *) &((iDynTree::FreeFloatingAcc const *)arg1)->jointAcc();
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *) &((iDynTree::FreeFloatingGeneralizedTorques const *)arg1)->jointTorques();
   _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -58590,50 +58811,50 @@ int _wrap_FreeFloatingAcc_jointAcc__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_FreeFloatingAcc_jointAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingGeneralizedTorques_jointTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingAcc_jointAcc__SWIG_0(resc,resv,argc,argv);
+      return _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_0(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_FreeFloatingAcc_jointAcc__SWIG_1(resc,resv,argc,argv);
+      return _wrap_FreeFloatingGeneralizedTorques_jointTorques__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingAcc_jointAcc'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingGeneralizedTorques_jointTorques'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::FreeFloatingAcc::jointAcc()\n"
-    "    iDynTree::FreeFloatingAcc::jointAcc() const\n");
+    "    iDynTree::FreeFloatingGeneralizedTorques::jointTorques()\n"
+    "    iDynTree::FreeFloatingGeneralizedTorques::jointTorques() const\n");
   return 1;
 }
 
 
-int _wrap_FreeFloatingAcc_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   unsigned int result;
   
-  if (!SWIG_check_num_args("FreeFloatingAcc_getNrOfDOFs",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingGeneralizedTorques_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingGeneralizedTorques_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
-  result = (unsigned int)((iDynTree::FreeFloatingAcc const *)arg1)->getNrOfDOFs();
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
+  result = (unsigned int)((iDynTree::FreeFloatingGeneralizedTorques const *)arg1)->getNrOfDOFs();
   _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -58642,22 +58863,22 @@ int _wrap_FreeFloatingAcc_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_delete_FreeFloatingAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+int _wrap_delete_FreeFloatingGeneralizedTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingGeneralizedTorques *arg1 = (iDynTree::FreeFloatingGeneralizedTorques *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_FreeFloatingAcc",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_FreeFloatingGeneralizedTorques",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingGeneralizedTorques" "', argument " "1"" of type '" "iDynTree::FreeFloatingGeneralizedTorques *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -58669,23 +58890,99 @@ int _wrap_delete_FreeFloatingAcc(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_ContactWrench_contactPoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+int _wrap_new_FreeFloatingVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::FreeFloatingVel *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_FreeFloatingVel",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::FreeFloatingVel *)new iDynTree::FreeFloatingVel();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingVel, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_FreeFloatingVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::FreeFloatingVel *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_FreeFloatingVel",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingVel" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingVel" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::FreeFloatingVel *)new iDynTree::FreeFloatingVel((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingVel, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_FreeFloatingVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_FreeFloatingVel__SWIG_0(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_FreeFloatingVel__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingVel'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::FreeFloatingVel::FreeFloatingVel()\n"
+    "    iDynTree::FreeFloatingVel::FreeFloatingVel(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_FreeFloatingVel_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Position *result = 0 ;
   
-  if (!SWIG_check_num_args("ContactWrench_contactPoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingVel_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactPoint" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
-  result = (iDynTree::Position *) &(arg1)->contactPoint();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Position, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingVel_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingVel_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58693,23 +58990,23 @@ int _wrap_ContactWrench_contactPoint__SWIG_0(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_ContactWrench_contactWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+int _wrap_FreeFloatingVel_baseVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  iDynTree::Twist *result = 0 ;
   
-  if (!SWIG_check_num_args("ContactWrench_contactWrench",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingVel_baseVel",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactWrench" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_baseVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
-  result = (iDynTree::Wrench *) &(arg1)->contactWrench();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  result = (iDynTree::Twist *) &(arg1)->baseVel();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58717,23 +59014,23 @@ int _wrap_ContactWrench_contactWrench__SWIG_0(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_ContactWrench_contactId(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+int _wrap_FreeFloatingVel_jointVel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned long *result = 0 ;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("ContactWrench_contactId",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingVel_jointVel",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactId" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_jointVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
-  result = (unsigned long *) &(arg1)->contactId();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_unsigned_long, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *) &(arg1)->jointVel();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58741,23 +59038,23 @@ int _wrap_ContactWrench_contactId(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_ContactWrench_contactPoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+int _wrap_FreeFloatingVel_baseVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Position *result = 0 ;
+  iDynTree::Twist *result = 0 ;
   
-  if (!SWIG_check_num_args("ContactWrench_contactPoint",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingVel_baseVel",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactPoint" "', argument " "1"" of type '" "iDynTree::ContactWrench const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_baseVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
-  result = (iDynTree::Position *) &((iDynTree::ContactWrench const *)arg1)->contactPoint();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Position, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  result = (iDynTree::Twist *) &((iDynTree::FreeFloatingVel const *)arg1)->baseVel();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Twist, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58765,51 +59062,51 @@ int _wrap_ContactWrench_contactPoint__SWIG_1(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_ContactWrench_contactPoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingVel_baseVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_ContactWrench_contactPoint__SWIG_0(resc,resv,argc,argv);
+      return _wrap_FreeFloatingVel_baseVel__SWIG_0(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_ContactWrench_contactPoint__SWIG_1(resc,resv,argc,argv);
+      return _wrap_FreeFloatingVel_baseVel__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ContactWrench_contactPoint'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingVel_baseVel'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ContactWrench::contactPoint()\n"
-    "    iDynTree::ContactWrench::contactPoint() const\n");
+    "    iDynTree::FreeFloatingVel::baseVel()\n"
+    "    iDynTree::FreeFloatingVel::baseVel() const\n");
   return 1;
 }
 
 
-int _wrap_ContactWrench_contactWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+int _wrap_FreeFloatingVel_jointVel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("ContactWrench_contactWrench",argc,1,1,0)) {
+  if (!SWIG_check_num_args("FreeFloatingVel_jointVel",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactWrench" "', argument " "1"" of type '" "iDynTree::ContactWrench const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_jointVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
-  result = (iDynTree::Wrench *) &((iDynTree::ContactWrench const *)arg1)->contactWrench();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *) &((iDynTree::FreeFloatingVel const *)arg1)->jointVel();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58817,44 +59114,51 @@ int _wrap_ContactWrench_contactWrench__SWIG_1(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_ContactWrench_contactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingVel_jointVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_ContactWrench_contactWrench__SWIG_0(resc,resv,argc,argv);
+      return _wrap_FreeFloatingVel_jointVel__SWIG_0(resc,resv,argc,argv);
     }
   }
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_ContactWrench_contactWrench__SWIG_1(resc,resv,argc,argv);
+      return _wrap_FreeFloatingVel_jointVel__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ContactWrench_contactWrench'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingVel_jointVel'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ContactWrench::contactWrench()\n"
-    "    iDynTree::ContactWrench::contactWrench() const\n");
+    "    iDynTree::FreeFloatingVel::jointVel()\n"
+    "    iDynTree::FreeFloatingVel::jointVel() const\n");
   return 1;
 }
 
 
-int _wrap_new_ContactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_FreeFloatingVel_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::ContactWrench *result = 0 ;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("new_ContactWrench",argc,0,0,0)) {
+  if (!SWIG_check_num_args("FreeFloatingVel_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::ContactWrench *)new iDynTree::ContactWrench();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ContactWrench, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingVel_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
+  result = (unsigned int)((iDynTree::FreeFloatingVel const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58862,22 +59166,22 @@ int _wrap_new_ContactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_delete_ContactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+int _wrap_delete_FreeFloatingVel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingVel *arg1 = (iDynTree::FreeFloatingVel *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_ContactWrench",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_FreeFloatingVel",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingVel, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ContactWrench" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingVel" "', argument " "1"" of type '" "iDynTree::FreeFloatingVel *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -58889,40 +59193,16 @@ int _wrap_delete_ContactWrench(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_new_LinkContactWrenches__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
-  mxArray * _out;
-  iDynTree::LinkContactWrenches *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_LinkContactWrenches",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkContactWrenches" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkContactWrenches *)new iDynTree::LinkContactWrenches(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkContactWrenches, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_LinkContactWrenches__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkContactWrenches *result = 0 ;
+  iDynTree::FreeFloatingAcc *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkContactWrenches",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingAcc",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::LinkContactWrenches *)new iDynTree::LinkContactWrenches();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkContactWrenches, 1 |  0 );
+  result = (iDynTree::FreeFloatingAcc *)new iDynTree::FreeFloatingAcc();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingAcc, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58930,26 +59210,26 @@ int _wrap_new_LinkContactWrenches__SWIG_1(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_new_LinkContactWrenches__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkContactWrenches *result = 0 ;
+  iDynTree::FreeFloatingAcc *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkContactWrenches",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_FreeFloatingAcc",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkContactWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_FreeFloatingAcc" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkContactWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_FreeFloatingAcc" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkContactWrenches *)new iDynTree::LinkContactWrenches((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkContactWrenches, 1 |  0 );
+  result = (iDynTree::FreeFloatingAcc *)new iDynTree::FreeFloatingAcc((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__FreeFloatingAcc, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -58957,9 +59237,9 @@ int _wrap_new_LinkContactWrenches__SWIG_2(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_new_LinkContactWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_FreeFloatingAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 0) {
-    return _wrap_new_LinkContactWrenches__SWIG_1(resc,resv,argc,argv);
+    return _wrap_new_FreeFloatingAcc__SWIG_0(resc,resv,argc,argv);
   }
   if (argc == 1) {
     int _v;
@@ -58967,52 +59247,44 @@ int _wrap_new_LinkContactWrenches(int resc, mxArray *resv[], int argc, mxArray *
     int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_LinkContactWrenches__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_LinkContactWrenches__SWIG_0(resc,resv,argc,argv);
+      return _wrap_new_FreeFloatingAcc__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkContactWrenches'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_FreeFloatingAcc'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkContactWrenches::LinkContactWrenches(unsigned int)\n"
-    "    iDynTree::LinkContactWrenches::LinkContactWrenches()\n"
-    "    iDynTree::LinkContactWrenches::LinkContactWrenches(iDynTree::Model const &)\n");
+    "    iDynTree::FreeFloatingAcc::FreeFloatingAcc()\n"
+    "    iDynTree::FreeFloatingAcc::FreeFloatingAcc(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_LinkContactWrenches_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  unsigned int arg2 ;
+int _wrap_FreeFloatingAcc_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FreeFloatingAcc_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_resize" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_resize" "', argument " "2"" of type '" "unsigned int""'");
-  } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "FreeFloatingAcc_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "FreeFloatingAcc_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -59021,33 +59293,47 @@ int _wrap_LinkContactWrenches_resize__SWIG_0(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_LinkContactWrenches_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_FreeFloatingAcc_baseAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
+  iDynTree::SpatialAcc *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FreeFloatingAcc_baseAcc",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_baseAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkContactWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  result = (iDynTree::SpatialAcc *) &(arg1)->baseAcc();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_FreeFloatingAcc_jointAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("FreeFloatingAcc_jointAcc",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkContactWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_jointAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *) &(arg1)->jointAcc();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59055,70 +59341,75 @@ int _wrap_LinkContactWrenches_resize__SWIG_1(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_LinkContactWrenches_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_FreeFloatingAcc_baseAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::SpatialAcc *result = 0 ;
+  
+  if (!SWIG_check_num_args("FreeFloatingAcc_baseAcc",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_baseAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  result = (iDynTree::SpatialAcc *) &((iDynTree::FreeFloatingAcc const *)arg1)->baseAcc();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SpatialAcc, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_FreeFloatingAcc_baseAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_LinkContactWrenches_resize__SWIG_1(resc,resv,argc,argv);
-      }
+      return _wrap_FreeFloatingAcc_baseAcc__SWIG_0(resc,resv,argc,argv);
     }
   }
-  if (argc == 2) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkContactWrenches_resize__SWIG_0(resc,resv,argc,argv);
-      }
+      return _wrap_FreeFloatingAcc_baseAcc__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkContactWrenches_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingAcc_baseAcc'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkContactWrenches::resize(unsigned int)\n"
-    "    iDynTree::LinkContactWrenches::resize(iDynTree::Model const &)\n");
+    "    iDynTree::FreeFloatingAcc::baseAcc()\n"
+    "    iDynTree::FreeFloatingAcc::baseAcc() const\n");
   return 1;
 }
 
 
-int _wrap_LinkContactWrenches_getNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_FreeFloatingAcc_jointAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_getNrOfContactsForLink",argc,2,2,0)) {
+  if (!SWIG_check_num_args("FreeFloatingAcc_jointAcc",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_getNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_jointAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_getNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = ((iDynTree::LinkContactWrenches const *)arg1)->getNrOfContactsForLink(arg2);
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *) &((iDynTree::FreeFloatingAcc const *)arg1)->jointAcc();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59126,37 +59417,77 @@ int _wrap_LinkContactWrenches_getNrOfContactsForLink(int resc, mxArray *resv[],
 }
 
 
-int _wrap_LinkContactWrenches_setNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  size_t arg3 ;
+int _wrap_FreeFloatingAcc_jointAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_FreeFloatingAcc_jointAcc__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_FreeFloatingAcc_jointAcc__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'FreeFloatingAcc_jointAcc'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::FreeFloatingAcc::jointAcc()\n"
+    "    iDynTree::FreeFloatingAcc::jointAcc() const\n");
+  return 1;
+}
+
+
+int _wrap_FreeFloatingAcc_getNrOfDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
+  unsigned int result;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_setNrOfContactsForLink",argc,3,3,0)) {
+  if (!SWIG_check_num_args("FreeFloatingAcc_getNrOfDOFs",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_setNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "FreeFloatingAcc_getNrOfDOFs" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  result = (unsigned int)((iDynTree::FreeFloatingAcc const *)arg1)->getNrOfDOFs();
+  _out = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_FreeFloatingAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::FreeFloatingAcc *arg1 = (iDynTree::FreeFloatingAcc *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_FreeFloatingAcc",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__FreeFloatingAcc, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_FreeFloatingAcc" "', argument " "1"" of type '" "iDynTree::FreeFloatingAcc *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_setNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkContactWrenches_setNrOfContactsForLink" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  (arg1)->setNrOfContactsForLink(arg2,arg3);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -59165,23 +59496,23 @@ int _wrap_LinkContactWrenches_setNrOfContactsForLink(int resc, mxArray *resv[],
 }
 
 
-int _wrap_LinkContactWrenches_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+int _wrap_ContactWrench_contactPoint__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::Position *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_getNrOfLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ContactWrench_contactPoint",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactPoint" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  result = ((iDynTree::LinkContactWrenches const *)arg1)->getNrOfLinks();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
+  result = (iDynTree::Position *) &(arg1)->contactPoint();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Position, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59189,39 +59520,23 @@ int _wrap_LinkContactWrenches_getNrOfLinks(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_LinkContactWrenches_contactWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  size_t arg3 ;
+int _wrap_ContactWrench_contactWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::ContactWrench *result = 0 ;
+  iDynTree::Wrench *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_contactWrench",argc,3,3,0)) {
+  if (!SWIG_check_num_args("ContactWrench_contactWrench",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactWrench" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkContactWrenches_contactWrench" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  result = (iDynTree::ContactWrench *) &(arg1)->contactWrench(arg2,arg3);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
+  result = (iDynTree::Wrench *) &(arg1)->contactWrench();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59229,39 +59544,23 @@ int _wrap_LinkContactWrenches_contactWrench__SWIG_0(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_LinkContactWrenches_contactWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  size_t arg3 ;
+int _wrap_ContactWrench_contactId(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::ContactWrench *result = 0 ;
+  unsigned long *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_contactWrench",argc,3,3,0)) {
+  if (!SWIG_check_num_args("ContactWrench_contactId",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactId" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkContactWrenches_contactWrench" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  result = (iDynTree::ContactWrench *) &((iDynTree::LinkContactWrenches const *)arg1)->contactWrench(arg2,arg3);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
+  result = (unsigned long *) &(arg1)->contactId();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_unsigned_long, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59269,86 +59568,75 @@ int _wrap_LinkContactWrenches_contactWrench__SWIG_1(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_LinkContactWrenches_contactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
+int _wrap_ContactWrench_contactPoint__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Position *result = 0 ;
+  
+  if (!SWIG_check_num_args("ContactWrench_contactPoint",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactPoint" "', argument " "1"" of type '" "iDynTree::ContactWrench const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
+  result = (iDynTree::Position *) &((iDynTree::ContactWrench const *)arg1)->contactPoint();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Position, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ContactWrench_contactPoint(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_size_t(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_LinkContactWrenches_contactWrench__SWIG_0(resc,resv,argc,argv);
-        }
-      }
+      return _wrap_ContactWrench_contactPoint__SWIG_0(resc,resv,argc,argv);
     }
   }
-  if (argc == 3) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_size_t(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_LinkContactWrenches_contactWrench__SWIG_1(resc,resv,argc,argv);
-        }
-      }
+      return _wrap_ContactWrench_contactPoint__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkContactWrenches_contactWrench'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ContactWrench_contactPoint'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkContactWrenches::contactWrench(iDynTree::LinkIndex const,size_t const)\n"
-    "    iDynTree::LinkContactWrenches::contactWrench(iDynTree::LinkIndex const,size_t const) const\n");
+    "    iDynTree::ContactWrench::contactPoint()\n"
+    "    iDynTree::ContactWrench::contactPoint() const\n");
   return 1;
 }
 
 
-int _wrap_LinkContactWrenches_computeNetWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::LinkNetExternalWrenches *arg2 = 0 ;
+int _wrap_ContactWrench_contactWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::Wrench *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_computeNetWrenches",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ContactWrench_contactWrench",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_computeNetWrenches" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkContactWrenches_computeNetWrenches" "', argument " "2"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkContactWrenches_computeNetWrenches" "', argument " "2"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ContactWrench_contactWrench" "', argument " "1"" of type '" "iDynTree::ContactWrench const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp2);
-  result = (bool)((iDynTree::LinkContactWrenches const *)arg1)->computeNetWrenches(*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
+  result = (iDynTree::Wrench *) &((iDynTree::ContactWrench const *)arg1)->contactWrench();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59356,34 +59644,44 @@ int _wrap_LinkContactWrenches_computeNetWrenches(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_LinkContactWrenches_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+int _wrap_ContactWrench_contactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_ContactWrench_contactWrench__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ContactWrench, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_ContactWrench_contactWrench__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ContactWrench_contactWrench'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ContactWrench::contactWrench()\n"
+    "    iDynTree::ContactWrench::contactWrench() const\n");
+  return 1;
+}
+
+
+int _wrap_new_ContactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  std::string result;
+  iDynTree::ContactWrench *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkContactWrenches_toString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_ContactWrench",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_toString" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkContactWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkContactWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::LinkContactWrenches const *)arg1)->toString((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  (void)argv;
+  result = (iDynTree::ContactWrench *)new iDynTree::ContactWrench();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ContactWrench, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59391,22 +59689,22 @@ int _wrap_LinkContactWrenches_toString(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_delete_LinkContactWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+int _wrap_delete_ContactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ContactWrench *arg1 = (iDynTree::ContactWrench *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_LinkContactWrenches",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_ContactWrench",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ContactWrench, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkContactWrenches" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ContactWrench" "', argument " "1"" of type '" "iDynTree::ContactWrench *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::ContactWrench * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -59418,70 +59716,131 @@ int _wrap_delete_LinkContactWrenches(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_ForwardPositionKinematics__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::Transform *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
-  iDynTree::LinkPositions *arg5 = 0 ;
-  void *argp1 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
+int _wrap_new_LinkContactWrenches__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::LinkContactWrenches *result = 0 ;
   
-  if (!SWIG_check_num_args("ForwardPositionKinematics",argc,5,5,0)) {
+  if (!SWIG_check_num_args("new_LinkContactWrenches",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkContactWrenches" "', argument " "1"" of type '" "unsigned int""'");
+  } 
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::LinkContactWrenches *)new iDynTree::LinkContactWrenches(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkContactWrenches, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkContactWrenches__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::LinkContactWrenches *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_LinkContactWrenches",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::LinkContactWrenches *)new iDynTree::LinkContactWrenches();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkContactWrenches, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkContactWrenches__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkContactWrenches *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_LinkContactWrenches",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkContactWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkContactWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+  result = (iDynTree::LinkContactWrenches *)new iDynTree::LinkContactWrenches((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkContactWrenches, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkContactWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_LinkContactWrenches__SWIG_1(resc,resv,argc,argv);
   }
-  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_LinkContactWrenches__SWIG_2(resc,resv,argc,argv);
+    }
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  if (argc == 1) {
+    int _v;
+    {
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      return _wrap_new_LinkContactWrenches__SWIG_0(resc,resv,argc,argv);
+    }
   }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardPositionKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkContactWrenches'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkContactWrenches::LinkContactWrenches(unsigned int)\n"
+    "    iDynTree::LinkContactWrenches::LinkContactWrenches()\n"
+    "    iDynTree::LinkContactWrenches::LinkContactWrenches(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkContactWrenches_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkContactWrenches_resize",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
-  result = (bool)iDynTree::ForwardPositionKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::Transform const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_resize" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59489,59 +59848,33 @@ int _wrap_ForwardPositionKinematics__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_ForwardPositionKinematics__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::FreeFloatingPos *arg3 = 0 ;
-  iDynTree::LinkPositions *arg4 = 0 ;
-  void *argp1 ;
+int _wrap_LinkContactWrenches_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ForwardPositionKinematics",argc,4,4,0)) {
+  if (!SWIG_check_num_args("LinkContactWrenches_resize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_resize" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkContactWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::LinkPositions &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkContactWrenches_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkPositions * >(argp4);
-  result = (bool)iDynTree::ForwardPositionKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59549,155 +59882,70 @@ int _wrap_ForwardPositionKinematics__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_ForwardPositionKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 4) {
+int _wrap_LinkContactWrenches_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_ForwardPositionKinematics__SWIG_1(resc,resv,argc,argv);
-          }
-        }
+        return _wrap_LinkContactWrenches_resize__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 5) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
         _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            void *vptr = 0;
-            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
-            _v = SWIG_CheckState(res);
-            if (_v) {
-              return _wrap_ForwardPositionKinematics__SWIG_0(resc,resv,argc,argv);
-            }
-          }
-        }
+      }
+      if (_v) {
+        return _wrap_LinkContactWrenches_resize__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ForwardPositionKinematics'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkContactWrenches_resize'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ForwardPositionKinematics(iDynTree::Model const &,iDynTree::Traversal const &,iDynTree::Transform const &,iDynTree::VectorDynSize const &,iDynTree::LinkPositions &)\n"
-    "    iDynTree::ForwardPositionKinematics(iDynTree::Model const &,iDynTree::Traversal const &,iDynTree::FreeFloatingPos const &,iDynTree::LinkPositions &)\n");
+    "    iDynTree::LinkContactWrenches::resize(unsigned int)\n"
+    "    iDynTree::LinkContactWrenches::resize(iDynTree::Model const &)\n");
   return 1;
 }
 
 
-int _wrap_ForwardVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::FreeFloatingPos *arg3 = 0 ;
-  iDynTree::FreeFloatingVel *arg4 = 0 ;
-  iDynTree::FreeFloatingAcc *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  iDynTree::LinkAccArray *arg7 = 0 ;
-  void *argp1 ;
+int _wrap_LinkContactWrenches_getNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("ForwardVelAccKinematics",argc,7,7,0)) {
+  if (!SWIG_check_num_args("LinkContactWrenches_getNrOfContactsForLink",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_getNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
   }
-  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
-  result = (bool)iDynTree::ForwardVelAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::FreeFloatingAcc const &)*arg5,*arg6,*arg7);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_getNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = ((iDynTree::LinkContactWrenches const *)arg1)->getNrOfContactsForLink(arg2);
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59705,103 +59953,38 @@ int _wrap_ForwardVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_ForwardPosVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::FreeFloatingPos *arg3 = 0 ;
-  iDynTree::FreeFloatingVel *arg4 = 0 ;
-  iDynTree::FreeFloatingAcc *arg5 = 0 ;
-  iDynTree::LinkPositions *arg6 = 0 ;
-  iDynTree::LinkVelArray *arg7 = 0 ;
-  iDynTree::LinkAccArray *arg8 = 0 ;
-  void *argp1 ;
+int _wrap_LinkContactWrenches_setNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
-  void *argp8 = 0 ;
-  int res8 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ForwardPosVelAccKinematics",argc,8,8,0)) {
+  if (!SWIG_check_num_args("LinkContactWrenches_setNrOfContactsForLink",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPosVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPosVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPosVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPosVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardPosVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardPosVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkPositions * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardPosVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::LinkVelArray * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "ForwardPosVelAccKinematics" "', argument " "8"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "8"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_setNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
   }
-  arg8 = reinterpret_cast< iDynTree::LinkAccArray * >(argp8);
-  result = (bool)iDynTree::ForwardPosVelAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::FreeFloatingAcc const &)*arg5,*arg6,*arg7,*arg8);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_setNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkContactWrenches_setNrOfContactsForLink" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  (arg1)->setNrOfContactsForLink(arg2,arg3);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59809,80 +59992,189 @@ int _wrap_ForwardPosVelAccKinematics(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_ForwardPosVelKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::FreeFloatingPos *arg3 = 0 ;
-  iDynTree::FreeFloatingVel *arg4 = 0 ;
-  iDynTree::LinkPositions *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  void *argp1 ;
+int _wrap_LinkContactWrenches_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("ForwardPosVelKinematics",argc,6,6,0)) {
+  if (!SWIG_check_num_args("LinkContactWrenches_getNrOfLinks",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPosVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPosVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPosVelKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  result = ((iDynTree::LinkContactWrenches const *)arg1)->getNrOfLinks();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkContactWrenches_contactWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  iDynTree::ContactWrench *result = 0 ;
+  
+  if (!SWIG_check_num_args("LinkContactWrenches_contactWrench",argc,3,3,0)) {
+    SWIG_fail;
   }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPosVelKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkContactWrenches_contactWrench" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  result = (iDynTree::ContactWrench *) &(arg1)->contactWrench(arg2,arg3);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkContactWrenches_contactWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  iDynTree::ContactWrench *result = 0 ;
+  
+  if (!SWIG_check_num_args("LinkContactWrenches_contactWrench",argc,3,3,0)) {
+    SWIG_fail;
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardPosVelKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkContactWrenches_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkContactWrenches_contactWrench" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  result = (iDynTree::ContactWrench *) &((iDynTree::LinkContactWrenches const *)arg1)->contactWrench(arg2,arg3);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ContactWrench, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkContactWrenches_contactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_LinkContactWrenches_contactWrench__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
   }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkContactWrenches, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_LinkContactWrenches_contactWrench__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
   }
-  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardPosVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkContactWrenches_contactWrench'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkContactWrenches::contactWrench(iDynTree::LinkIndex const,size_t const)\n"
+    "    iDynTree::LinkContactWrenches::contactWrench(iDynTree::LinkIndex const,size_t const) const\n");
+  return 1;
+}
+
+
+int _wrap_LinkContactWrenches_computeNetWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::LinkNetExternalWrenches *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("LinkContactWrenches_computeNetWrenches",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_computeNetWrenches" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  result = (bool)iDynTree::ForwardPosVelKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,*arg5,*arg6);
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkContactWrenches_computeNetWrenches" "', argument " "2"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkContactWrenches_computeNetWrenches" "', argument " "2"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp2);
+  result = (bool)((iDynTree::LinkContactWrenches const *)arg1)->computeNetWrenches(*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -59891,92 +60183,61 @@ int _wrap_ForwardPosVelKinematics(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_ForwardAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::FreeFloatingPos *arg3 = 0 ;
-  iDynTree::FreeFloatingVel *arg4 = 0 ;
-  iDynTree::FreeFloatingAcc *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  iDynTree::LinkAccArray *arg7 = 0 ;
-  void *argp1 ;
+int _wrap_LinkContactWrenches_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
   mxArray * _out;
-  bool result;
+  std::string result;
   
-  if (!SWIG_check_num_args("ForwardAccKinematics",argc,7,7,0)) {
+  if (!SWIG_check_num_args("LinkContactWrenches_toString",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkContactWrenches_toString" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkContactWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkContactWrenches_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = ((iDynTree::LinkContactWrenches const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_LinkContactWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkContactWrenches *arg1 = (iDynTree::LinkContactWrenches *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_LinkContactWrenches",argc,1,1,0)) {
+    SWIG_fail;
   }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkContactWrenches, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkContactWrenches" "', argument " "1"" of type '" "iDynTree::LinkContactWrenches *""'"); 
   }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  arg1 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
-  result = (bool)iDynTree::ForwardAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::FreeFloatingAcc const &)*arg5,(iDynTree::LinkVelArray const &)*arg6,*arg7);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -59984,13 +60245,12 @@ int _wrap_ForwardAccKinematics(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_ForwardBiasAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ForwardPositionKinematics__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::FreeFloatingPos *arg3 = 0 ;
-  iDynTree::FreeFloatingVel *arg4 = 0 ;
-  iDynTree::LinkVelArray *arg5 = 0 ;
-  iDynTree::LinkAccArray *arg6 = 0 ;
+  iDynTree::Transform *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  iDynTree::LinkPositions *arg5 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -59999,65 +60259,55 @@ int _wrap_ForwardBiasAccKinematics(int resc, mxArray *resv[], int argc, mxArray
   int res3 = 0 ;
   void *argp4 ;
   int res4 = 0 ;
-  void *argp5 ;
+  void *argp5 = 0 ;
   int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ForwardBiasAccKinematics",argc,6,6,0)) {
+  if (!SWIG_check_num_args("ForwardPositionKinematics",argc,5,5,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardBiasAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardBiasAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardBiasAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardBiasAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardBiasAccKinematics" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardPositionKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardBiasAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
-  result = (bool)iDynTree::ForwardBiasAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::LinkVelArray const &)*arg5,*arg6);
+  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
+  result = (bool)iDynTree::ForwardPositionKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::Transform const &)*arg3,(iDynTree::VectorDynSize const &)*arg4,*arg5);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -60066,11 +60316,11 @@ int _wrap_ForwardBiasAccKinematics(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_ComputeLinearAndAngularMomentum(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ForwardPositionKinematics__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
-  iDynTree::LinkPositions *arg2 = 0 ;
-  iDynTree::LinkVelArray *arg3 = 0 ;
-  iDynTree::SpatialMomentum *arg4 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::LinkPositions *arg4 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -60082,42 +60332,42 @@ int _wrap_ComputeLinearAndAngularMomentum(int resc, mxArray *resv[], int argc, m
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ComputeLinearAndAngularMomentum",argc,4,4,0)) {
+  if (!SWIG_check_num_args("ForwardPositionKinematics",argc,4,4,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkPositions * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::LinkVelArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__SpatialMomentum,  0 );
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "4"" of type '" "iDynTree::SpatialMomentum &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "4"" of type '" "iDynTree::SpatialMomentum &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPositionKinematics" "', argument " "4"" of type '" "iDynTree::LinkPositions &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::SpatialMomentum * >(argp4);
-  result = (bool)iDynTree::ComputeLinearAndAngularMomentum((iDynTree::Model const &)*arg1,(iDynTree::LinkPositions const &)*arg2,(iDynTree::LinkVelArray const &)*arg3,*arg4);
+  arg4 = reinterpret_cast< iDynTree::LinkPositions * >(argp4);
+  result = (bool)iDynTree::ForwardPositionKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -60126,12 +60376,77 @@ int _wrap_ComputeLinearAndAngularMomentum(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_ComputeLinearAndAngularMomentumDerivativeBias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ForwardPositionKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ForwardPositionKinematics__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__LinkPositions, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_ForwardPositionKinematics__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ForwardPositionKinematics'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ForwardPositionKinematics(iDynTree::Model const &,iDynTree::Traversal const &,iDynTree::Transform const &,iDynTree::VectorDynSize const &,iDynTree::LinkPositions &)\n"
+    "    iDynTree::ForwardPositionKinematics(iDynTree::Model const &,iDynTree::Traversal const &,iDynTree::FreeFloatingPos const &,iDynTree::LinkPositions &)\n");
+  return 1;
+}
+
+
+int _wrap_ForwardVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
-  iDynTree::LinkPositions *arg2 = 0 ;
-  iDynTree::LinkVelArray *arg3 = 0 ;
-  iDynTree::LinkAccArray *arg4 = 0 ;
-  iDynTree::Wrench *arg5 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::FreeFloatingVel *arg4 = 0 ;
+  iDynTree::FreeFloatingAcc *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  iDynTree::LinkAccArray *arg7 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -60140,72 +60455,92 @@ int _wrap_ComputeLinearAndAngularMomentumDerivativeBias(int resc, mxArray *resv[
   int res3 = 0 ;
   void *argp4 ;
   int res4 = 0 ;
-  void *argp5 = 0 ;
+  void *argp5 ;
   int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ComputeLinearAndAngularMomentumDerivativeBias",argc,5,5,0)) {
+  if (!SWIG_check_num_args("ForwardVelAccKinematics",argc,7,7,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkPositions * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::LinkVelArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "4"" of type '" "iDynTree::LinkAccArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "4"" of type '" "iDynTree::LinkAccArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkAccArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__Wrench,  0 );
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "5"" of type '" "iDynTree::Wrench &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "5"" of type '" "iDynTree::Wrench &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::Wrench * >(argp5);
-  result = (bool)iDynTree::ComputeLinearAndAngularMomentumDerivativeBias((iDynTree::Model const &)*arg1,(iDynTree::LinkPositions const &)*arg2,(iDynTree::LinkVelArray const &)*arg3,(iDynTree::LinkAccArray const &)*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  arg5 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
+  result = (bool)iDynTree::ForwardVelAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::FreeFloatingAcc const &)*arg5,*arg6,*arg7);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_RNEADynamicPhase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ForwardPosVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::JointPosDoubleArray *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::LinkAccArray *arg5 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg6 = 0 ;
-  iDynTree::LinkInternalWrenches *arg7 = 0 ;
-  iDynTree::FreeFloatingGeneralizedTorques *arg8 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::FreeFloatingVel *arg4 = 0 ;
+  iDynTree::FreeFloatingAcc *arg5 = 0 ;
+  iDynTree::LinkPositions *arg6 = 0 ;
+  iDynTree::LinkVelArray *arg7 = 0 ;
+  iDynTree::LinkAccArray *arg8 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -60216,7 +60551,7 @@ int _wrap_RNEADynamicPhase(int resc, mxArray *resv[], int argc, mxArray *argv[])
   int res4 = 0 ;
   void *argp5 ;
   int res5 = 0 ;
-  void *argp6 ;
+  void *argp6 = 0 ;
   int res6 = 0 ;
   void *argp7 = 0 ;
   int res7 = 0 ;
@@ -60225,74 +60560,74 @@ int _wrap_RNEADynamicPhase(int resc, mxArray *resv[], int argc, mxArray *argv[])
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("RNEADynamicPhase",argc,8,8,0)) {
+  if (!SWIG_check_num_args("ForwardPosVelAccKinematics",argc,8,8,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RNEADynamicPhase" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPosVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RNEADynamicPhase" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPosVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RNEADynamicPhase" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPosVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RNEADynamicPhase" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPosVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RNEADynamicPhase" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardPosVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  arg5 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RNEADynamicPhase" "', argument " "6"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardPosVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "6"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkPositions &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  arg6 = reinterpret_cast< iDynTree::LinkPositions * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "RNEADynamicPhase" "', argument " "7"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardPosVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
   if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "7"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
-  arg7 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques,  0 );
+  arg7 = reinterpret_cast< iDynTree::LinkVelArray * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "RNEADynamicPhase" "', argument " "8"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "ForwardPosVelAccKinematics" "', argument " "8"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
   if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "8"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelAccKinematics" "', argument " "8"" of type '" "iDynTree::LinkAccArray &""'"); 
   }
-  arg8 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp8);
-  result = (bool)iDynTree::RNEADynamicPhase((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::JointPosDoubleArray const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::LinkAccArray const &)*arg5,(iDynTree::LinkWrenches const &)*arg6,*arg7,*arg8);
+  arg8 = reinterpret_cast< iDynTree::LinkAccArray * >(argp8);
+  result = (bool)iDynTree::ForwardPosVelAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::FreeFloatingAcc const &)*arg5,*arg6,*arg7,*arg8);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -60301,87 +60636,81 @@ int _wrap_RNEADynamicPhase(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_CompositeRigidBodyAlgorithm(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ForwardPosVelKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
   iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::JointPosDoubleArray *arg3 = 0 ;
-  iDynTree::LinkCompositeRigidBodyInertias *arg4 = 0 ;
-  iDynTree::FreeFloatingMassMatrix *arg5 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::FreeFloatingVel *arg4 = 0 ;
+  iDynTree::LinkPositions *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
   void *argp3 ;
   int res3 = 0 ;
-  void *argp4 = 0 ;
+  void *argp4 ;
   int res4 = 0 ;
   void *argp5 = 0 ;
   int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("CompositeRigidBodyAlgorithm",argc,5,5,0)) {
+  if (!SWIG_check_num_args("ForwardPosVelKinematics",argc,6,6,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardPosVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardPosVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardPosVelKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkInertias,  0 );
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "4"" of type '" "iDynTree::LinkCompositeRigidBodyInertias &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardPosVelKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "4"" of type '" "iDynTree::LinkCompositeRigidBodyInertias &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkCompositeRigidBodyInertias * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix,  0 );
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "5"" of type '" "iDynTree::FreeFloatingMassMatrix &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardPosVelKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "5"" of type '" "iDynTree::FreeFloatingMassMatrix &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "5"" of type '" "iDynTree::LinkPositions &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::FreeFloatingMassMatrix * >(argp5);
-  result = (bool)iDynTree::CompositeRigidBodyAlgorithm((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::JointPosDoubleArray const &)*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_ArticulatedBodyAlgorithmInternalBuffers",argc,0,0,0)) {
-    SWIG_fail;
+  arg5 = reinterpret_cast< iDynTree::LinkPositions * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardPosVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
   }
-  (void)argv;
-  result = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *)new iDynTree::ArticulatedBodyAlgorithmInternalBuffers();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 1 |  0 );
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardPosVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  result = (bool)iDynTree::ForwardPosVelKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,*arg5,*arg6);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60389,82 +60718,92 @@ int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_0(int resc, mxArray
 }
 
 
-int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ForwardAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::FreeFloatingVel *arg4 = 0 ;
+  iDynTree::FreeFloatingAcc *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  iDynTree::LinkAccArray *arg7 = 0 ;
   void *argp1 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
   mxArray * _out;
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_ArticulatedBodyAlgorithmInternalBuffers",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ForwardAccKinematics",argc,7,7,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_ArticulatedBodyAlgorithmInternalBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_ArticulatedBodyAlgorithmInternalBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *)new iDynTree::ArticulatedBodyAlgorithmInternalBuffers((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_0(resc,resv,argc,argv);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_1(resc,resv,argc,argv);
-    }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_ArticulatedBodyAlgorithmInternalBuffers'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ArticulatedBodyAlgorithmInternalBuffers::ArticulatedBodyAlgorithmInternalBuffers()\n"
-    "    iDynTree::ArticulatedBodyAlgorithmInternalBuffers::ArticulatedBodyAlgorithmInternalBuffers(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_resize",argc,2,2,0)) {
-    SWIG_fail;
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_resize" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ArticulatedBodyAlgorithmInternalBuffers_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
-  _out = (mxArray*)0;
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "5"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
+  result = (bool)iDynTree::ForwardAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::FreeFloatingAcc const &)*arg5,(iDynTree::LinkVelArray const &)*arg6,*arg7);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60472,33 +60811,91 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(int resc, mxArray *resv
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  void *argp1 = 0 ;
+int _wrap_ForwardBiasAccKinematics__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::FreeFloatingVel *arg4 = 0 ;
+  iDynTree::SpatialAcc *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  iDynTree::LinkAccArray *arg7 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ForwardBiasAccKinematics",argc,7,7,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_isConsistent" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardBiasAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardBiasAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ArticulatedBodyAlgorithmInternalBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)(arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardBiasAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardBiasAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__SpatialAcc,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardBiasAccKinematics" "', argument " "5"" of type '" "iDynTree::SpatialAcc const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "5"" of type '" "iDynTree::SpatialAcc const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::SpatialAcc * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardBiasAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ForwardBiasAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "7"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
+  result = (bool)iDynTree::ForwardBiasAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::SpatialAcc const &)*arg5,(iDynTree::LinkVelArray const &)*arg6,*arg7);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -60507,85 +60904,81 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(int resc, mxArray
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_S_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::DOFSpatialMotionArray *arg2 = (iDynTree::DOFSpatialMotionArray *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_ForwardBiasAccKinematics__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::FreeFloatingPos *arg3 = 0 ;
+  iDynTree::FreeFloatingVel *arg4 = 0 ;
+  iDynTree::LinkVelArray *arg5 = 0 ;
+  iDynTree::LinkAccArray *arg6 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_S_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ForwardBiasAccKinematics",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_S_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ForwardBiasAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_S_set" "', argument " "2"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ForwardBiasAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp2);
-  if (arg1) (arg1)->S = *arg2;
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_S_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::DOFSpatialMotionArray *result = 0 ;
-  
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_S_get",argc,1,1,0)) {
-    SWIG_fail;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_S_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ForwardBiasAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::DOFSpatialMotionArray *)& ((arg1)->S);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::DOFSpatialForceArray *arg2 = (iDynTree::DOFSpatialForceArray *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_U_set",argc,2,2,0)) {
-    SWIG_fail;
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "3"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_U_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ForwardBiasAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_U_set" "', argument " "2"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "4"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp2);
-  if (arg1) (arg1)->U = *arg2;
-  _out = (mxArray*)0;
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ForwardBiasAccKinematics" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ForwardBiasAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ForwardBiasAccKinematics" "', argument " "6"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
+  result = (bool)iDynTree::ForwardBiasAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::FreeFloatingPos const &)*arg3,(iDynTree::FreeFloatingVel const &)*arg4,(iDynTree::LinkVelArray const &)*arg5,*arg6);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60593,54 +60986,142 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(int resc, mxArray *resv[
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_U_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::DOFSpatialForceArray *result = 0 ;
-  
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_U_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_U_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::DOFSpatialForceArray *)& ((arg1)->U);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+int _wrap_ForwardBiasAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 6) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              void *vptr = 0;
+              int res = SWIG_ConvertPtr(argv[5], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+              _v = SWIG_CheckState(res);
+              if (_v) {
+                return _wrap_ForwardBiasAccKinematics__SWIG_1(resc,resv,argc,argv);
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+  if (argc == 7) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Traversal, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingPos, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__FreeFloatingVel, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__SpatialAcc, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              void *vptr = 0;
+              int res = SWIG_ConvertPtr(argv[5], &vptr, SWIGTYPE_p_iDynTree__LinkVelArray, 0);
+              _v = SWIG_CheckState(res);
+              if (_v) {
+                void *vptr = 0;
+                int res = SWIG_ConvertPtr(argv[6], &vptr, SWIGTYPE_p_iDynTree__LinkAccArray, 0);
+                _v = SWIG_CheckState(res);
+                if (_v) {
+                  return _wrap_ForwardBiasAccKinematics__SWIG_0(resc,resv,argc,argv);
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ForwardBiasAccKinematics'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ForwardBiasAccKinematics(iDynTree::Model const &,iDynTree::Traversal const &,iDynTree::FreeFloatingPos const &,iDynTree::FreeFloatingVel const &,iDynTree::SpatialAcc const &,iDynTree::LinkVelArray const &,iDynTree::LinkAccArray &)\n"
+    "    iDynTree::ForwardBiasAccKinematics(iDynTree::Model const &,iDynTree::Traversal const &,iDynTree::FreeFloatingPos const &,iDynTree::FreeFloatingVel const &,iDynTree::LinkVelArray const &,iDynTree::LinkAccArray &)\n");
   return 1;
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::JointDOFsDoubleArray *arg2 = (iDynTree::JointDOFsDoubleArray *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_ComputeLinearAndAngularMomentum(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::LinkPositions *arg2 = 0 ;
+  iDynTree::LinkVelArray *arg3 = 0 ;
+  iDynTree::SpatialMomentum *arg4 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_D_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ComputeLinearAndAngularMomentum",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_D_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_D_set" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp2);
-  if (arg1) (arg1)->D = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkPositions * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::LinkVelArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__SpatialMomentum,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ComputeLinearAndAngularMomentum" "', argument " "4"" of type '" "iDynTree::SpatialMomentum &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentum" "', argument " "4"" of type '" "iDynTree::SpatialMomentum &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::SpatialMomentum * >(argp4);
+  result = (bool)iDynTree::ComputeLinearAndAngularMomentum((iDynTree::Model const &)*arg1,(iDynTree::LinkPositions const &)*arg2,(iDynTree::LinkVelArray const &)*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60648,23 +61129,70 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(int resc, mxArray *resv[
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_ComputeLinearAndAngularMomentumDerivativeBias(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::LinkPositions *arg2 = 0 ;
+  iDynTree::LinkVelArray *arg3 = 0 ;
+  iDynTree::LinkAccArray *arg4 = 0 ;
+  iDynTree::Wrench *arg5 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_D_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ComputeLinearAndAngularMomentumDerivativeBias",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_D_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *)& ((arg1)->D);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "2"" of type '" "iDynTree::LinkPositions const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkPositions * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "3"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::LinkVelArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "4"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "4"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkAccArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__Wrench,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "5"" of type '" "iDynTree::Wrench &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ComputeLinearAndAngularMomentumDerivativeBias" "', argument " "5"" of type '" "iDynTree::Wrench &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Wrench * >(argp5);
+  result = (bool)iDynTree::ComputeLinearAndAngularMomentumDerivativeBias((iDynTree::Model const &)*arg1,(iDynTree::LinkPositions const &)*arg2,(iDynTree::LinkVelArray const &)*arg3,(iDynTree::LinkAccArray const &)*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60672,30 +61200,103 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(int resc, mxArray *resv[
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::JointDOFsDoubleArray *arg2 = (iDynTree::JointDOFsDoubleArray *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_RNEADynamicPhase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::JointPosDoubleArray *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkAccArray *arg5 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg6 = 0 ;
+  iDynTree::LinkInternalWrenches *arg7 = 0 ;
+  iDynTree::FreeFloatingGeneralizedTorques *arg8 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
+  void *argp8 = 0 ;
+  int res8 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_u_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("RNEADynamicPhase",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_u_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "RNEADynamicPhase" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_u_set" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "RNEADynamicPhase" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp2);
-  if (arg1) (arg1)->u = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "RNEADynamicPhase" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "RNEADynamicPhase" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "RNEADynamicPhase" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "RNEADynamicPhase" "', argument " "6"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "6"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "RNEADynamicPhase" "', argument " "7"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "7"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "RNEADynamicPhase" "', argument " "8"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "RNEADynamicPhase" "', argument " "8"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp8);
+  result = (bool)iDynTree::RNEADynamicPhase((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::JointPosDoubleArray const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::LinkAccArray const &)*arg5,(iDynTree::LinkWrenches const &)*arg6,*arg7,*arg8);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60703,23 +61304,70 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(int resc, mxArray *resv[
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_CompositeRigidBodyAlgorithm(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::JointPosDoubleArray *arg3 = 0 ;
+  iDynTree::LinkCompositeRigidBodyInertias *arg4 = 0 ;
+  iDynTree::FreeFloatingMassMatrix *arg5 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
-  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_u_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("CompositeRigidBodyAlgorithm",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_u_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::JointDOFsDoubleArray *)& ((arg1)->u);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "3"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkInertias,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "4"" of type '" "iDynTree::LinkCompositeRigidBodyInertias &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "4"" of type '" "iDynTree::LinkCompositeRigidBodyInertias &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkCompositeRigidBodyInertias * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingMassMatrix,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "CompositeRigidBodyAlgorithm" "', argument " "5"" of type '" "iDynTree::FreeFloatingMassMatrix &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "CompositeRigidBodyAlgorithm" "', argument " "5"" of type '" "iDynTree::FreeFloatingMassMatrix &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::FreeFloatingMassMatrix * >(argp5);
+  result = (bool)iDynTree::CompositeRigidBodyAlgorithm((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::JointPosDoubleArray const &)*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60727,30 +61375,16 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(int resc, mxArray *resv[
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::LinkVelArray *arg2 = (iDynTree::LinkVelArray *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *result = 0 ;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksVel_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_ArticulatedBodyAlgorithmInternalBuffers",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set" "', argument " "2"" of type '" "iDynTree::LinkVelArray *""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::LinkVelArray * >(argp2);
-  if (arg1) (arg1)->linksVel = *arg2;
-  _out = (mxArray*)0;
+  (void)argv;
+  result = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *)new iDynTree::ArticulatedBodyAlgorithmInternalBuffers();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60758,23 +61392,26 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(int resc, mxArray
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  void *argp1 = 0 ;
+int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkVelArray *result = 0 ;
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *result = 0 ;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksVel_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_ArticulatedBodyAlgorithmInternalBuffers",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksVel_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_ArticulatedBodyAlgorithmInternalBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::LinkVelArray *)& ((arg1)->linksVel);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_ArticulatedBodyAlgorithmInternalBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *)new iDynTree::ArticulatedBodyAlgorithmInternalBuffers((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60782,29 +61419,54 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(int resc, mxArray
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_ArticulatedBodyAlgorithmInternalBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_0(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_ArticulatedBodyAlgorithmInternalBuffers__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_ArticulatedBodyAlgorithmInternalBuffers'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ArticulatedBodyAlgorithmInternalBuffers::ArticulatedBodyAlgorithmInternalBuffers()\n"
+    "    iDynTree::ArticulatedBodyAlgorithmInternalBuffers::ArticulatedBodyAlgorithmInternalBuffers(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::LinkAccArray *arg2 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_resize",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_resize" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set" "', argument " "2"" of type '" "iDynTree::LinkAccArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkAccArray * >(argp2);
-  if (arg1) (arg1)->linksBiasAcceleration = *arg2;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ArticulatedBodyAlgorithmInternalBuffers_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -60813,23 +61475,34 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(int
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkAccArray *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_isConsistent",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_isConsistent" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::LinkAccArray *)& ((arg1)->linksBiasAcceleration);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ArticulatedBodyAlgorithmInternalBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)(arg1)->isConsistent((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60837,29 +61510,29 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(int
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_S_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::LinkAccArray *arg2 = (iDynTree::LinkAccArray *) 0 ;
+  iDynTree::DOFSpatialMotionArray *arg2 = (iDynTree::DOFSpatialMotionArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_S_set",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_S_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set" "', argument " "2"" of type '" "iDynTree::LinkAccArray *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_S_set" "', argument " "2"" of type '" "iDynTree::DOFSpatialMotionArray *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkAccArray * >(argp2);
-  if (arg1) (arg1)->linksAccelerations = *arg2;
+  arg2 = reinterpret_cast< iDynTree::DOFSpatialMotionArray * >(argp2);
+  if (arg1) (arg1)->S = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -60868,23 +61541,23 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(int res
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_S_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkAccArray *result = 0 ;
+  iDynTree::DOFSpatialMotionArray *result = 0 ;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_S_get",argc,1,1,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_S_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
-  result = (iDynTree::LinkAccArray *)& ((arg1)->linksAccelerations);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  result = (iDynTree::DOFSpatialMotionArray *)& ((arg1)->S);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialMotionArray, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -60892,21 +61565,351 @@ int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(int res
 }
 
 
-int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
-  iDynTree::LinkArticulatedBodyInertias *arg2 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  iDynTree::DOFSpatialForceArray *arg2 = (iDynTree::DOFSpatialForceArray *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_U_set",argc,2,2,0)) {
     SWIG_fail;
   }
   res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_U_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_U_set" "', argument " "2"" of type '" "iDynTree::DOFSpatialForceArray *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::DOFSpatialForceArray * >(argp2);
+  if (arg1) (arg1)->U = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_U_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::DOFSpatialForceArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_U_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_U_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  result = (iDynTree::DOFSpatialForceArray *)& ((arg1)->U);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__DOFSpatialForceArray, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::JointDOFsDoubleArray *arg2 = (iDynTree::JointDOFsDoubleArray *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_D_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_D_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_D_set" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp2);
+  if (arg1) (arg1)->D = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_D_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_D_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *)& ((arg1)->D);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::JointDOFsDoubleArray *arg2 = (iDynTree::JointDOFsDoubleArray *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_u_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_u_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_u_set" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp2);
+  if (arg1) (arg1)->u = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::JointDOFsDoubleArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_u_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_u_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  result = (iDynTree::JointDOFsDoubleArray *)& ((arg1)->u);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__JointDOFsDoubleArray, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::LinkVelArray *arg2 = (iDynTree::LinkVelArray *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksVel_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set" "', argument " "2"" of type '" "iDynTree::LinkVelArray *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkVelArray * >(argp2);
+  if (arg1) (arg1)->linksVel = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkVelArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksVel_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksVel_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  result = (iDynTree::LinkVelArray *)& ((arg1)->linksVel);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkVelArray, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::LinkAccArray *arg2 = (iDynTree::LinkAccArray *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set" "', argument " "2"" of type '" "iDynTree::LinkAccArray *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkAccArray * >(argp2);
+  if (arg1) (arg1)->linksBiasAcceleration = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkAccArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  result = (iDynTree::LinkAccArray *)& ((arg1)->linksBiasAcceleration);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::LinkAccArray *arg2 = (iDynTree::LinkAccArray *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set" "', argument " "2"" of type '" "iDynTree::LinkAccArray *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkAccArray * >(argp2);
+  if (arg1) (arg1)->linksAccelerations = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkAccArray *result = 0 ;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
+  result = (iDynTree::LinkAccArray *)& ((arg1)->linksAccelerations);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkAccArray, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ArticulatedBodyAlgorithmInternalBuffers *arg1 = (iDynTree::ArticulatedBodyAlgorithmInternalBuffers *) 0 ;
+  iDynTree::LinkArticulatedBodyInertias *arg2 = (iDynTree::LinkArticulatedBodyInertias *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set" "', argument " "1"" of type '" "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *""'"); 
   }
   arg1 = reinterpret_cast< iDynTree::ArticulatedBodyAlgorithmInternalBuffers * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkArticulatedBodyInertias, 0 |  0 );
@@ -61133,6 +62136,88 @@ int _wrap_ArticulatedBodyAlgorithm(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
+int _wrap_InverseDynamicsInertialParametersRegressor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::LinkPositions *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkAccArray *arg5 = 0 ;
+  iDynTree::MatrixDynSize *arg6 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("InverseDynamicsInertialParametersRegressor",argc,6,6,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "3"" of type '" "iDynTree::LinkPositions const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "3"" of type '" "iDynTree::LinkPositions const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::LinkPositions * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "6"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "InverseDynamicsInertialParametersRegressor" "', argument " "6"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp6);
+  result = (bool)iDynTree::InverseDynamicsInertialParametersRegressor((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::LinkPositions const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::LinkAccArray const &)*arg5,*arg6);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 SWIGINTERN int _wrap_NR_OF_SENSOR_TYPES_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   resv[0] = SWIG_From_int(static_cast< int >(iDynTree::NR_OF_SENSOR_TYPES));
   return 0;
@@ -66488,60 +67573,5040 @@ int _wrap_ThreeAxisForceTorqueContactSensor_setName(int resc, mxArray *resv[], i
   if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setName",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setName" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setName" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setName" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setName" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->setName((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  iDynTree::Transform *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setLinkSensorTransform",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
+  result = (bool)(arg1)->setLinkSensorTransform((iDynTree::Transform const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_setParentLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setParentLink",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLink" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setParentLink" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->setParentLink((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  iDynTree::LinkIndex *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  iDynTree::LinkIndex temp2 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setParentLinkIndex",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLinkIndex" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  temp2 = static_cast< iDynTree::LinkIndex >(val2);
+  arg2 = &temp2;
+  result = (bool)(arg1)->setParentLinkIndex((iDynTree::LinkIndex const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_getName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::string result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getName",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getName" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getName();
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_getSensorType(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::SensorType result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getSensorType",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getSensorType" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = (iDynTree::SensorType)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getSensorType();
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_getParentLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::string result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getParentLink",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getParentLink" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getParentLink();
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkIndex result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getParentLinkIndex",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getParentLinkIndex" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = (iDynTree::LinkIndex)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getParentLinkIndex();
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_getLinkSensorTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Transform result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getLinkSensorTransform",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getLinkSensorTransform" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getLinkSensorTransform();
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_isValid",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_isValid" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = (bool)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->isValid();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Sensor *result = 0 ;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_clone",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_clone" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = (iDynTree::Sensor *)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->clone();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sensor, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_updateIndices(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_updateIndices",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_updateIndices" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_updateIndices" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_updateIndices" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)(arg1)->updateIndices((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_setLoadCellLocations(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setLoadCellLocations",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setLoadCellLocations" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setLoadCellLocations" "', argument " "2"" of type '" "std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setLoadCellLocations" "', argument " "2"" of type '" "std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > &""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > * >(argp2);
+  (arg1)->setLoadCellLocations(*arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  SwigValueWrapper< std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > > result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getLoadCellLocations",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getLoadCellLocations" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getLoadCellLocations();
+  _out = SWIG_NewPointerObj((new std::vector< iDynTree::Position,std::allocator< iDynTree::Position > >(static_cast< const std::vector< iDynTree::Position,std::allocator< iDynTree::Position > >& >(result))), SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  iDynTree::Vector3 result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->computeThreeAxisForceTorqueFromLoadCellMeasurements(*arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Vector3(static_cast< const iDynTree::Vector3& >(result))), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  iDynTree::Position result;
+  
+  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->computeCenterOfPressureFromLoadCellMeasurements(*arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Position(static_cast< const iDynTree::Position& >(result))), SWIGTYPE_p_iDynTree__Position, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_predictSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::SensorsList *arg2 = 0 ;
+  iDynTree::Traversal *arg3 = 0 ;
+  iDynTree::FreeFloatingPos *arg4 = 0 ;
+  iDynTree::FreeFloatingVel *arg5 = 0 ;
+  iDynTree::FreeFloatingAcc *arg6 = 0 ;
+  iDynTree::LinAcceleration *arg7 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg8 = 0 ;
+  iDynTree::FreeFloatingAcc *arg9 = 0 ;
+  iDynTree::LinkPositions *arg10 = 0 ;
+  iDynTree::LinkVelArray *arg11 = 0 ;
+  iDynTree::LinkAccArray *arg12 = 0 ;
+  iDynTree::LinkInternalWrenches *arg13 = 0 ;
+  iDynTree::FreeFloatingGeneralizedTorques *arg14 = 0 ;
+  iDynTree::SensorsMeasurements *arg15 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 ;
+  int res7 = 0 ;
+  void *argp8 ;
+  int res8 = 0 ;
+  void *argp9 = 0 ;
+  int res9 = 0 ;
+  void *argp10 = 0 ;
+  int res10 = 0 ;
+  void *argp11 = 0 ;
+  int res11 = 0 ;
+  void *argp12 = 0 ;
+  int res12 = 0 ;
+  void *argp13 = 0 ;
+  int res13 = 0 ;
+  void *argp14 = 0 ;
+  int res14 = 0 ;
+  void *argp15 = 0 ;
+  int res15 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("predictSensorsMeasurements",argc,15,15,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "predictSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "predictSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "predictSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Traversal * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "predictSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "predictSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "predictSensorsMeasurements" "', argument " "6"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "6"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinearMotionVector3,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "predictSensorsMeasurements" "', argument " "7"" of type '" "iDynTree::LinAcceleration const &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "7"" of type '" "iDynTree::LinAcceleration const &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinAcceleration * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "predictSensorsMeasurements" "', argument " "8"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "8"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp8);
+  res9 = SWIG_ConvertPtr(argv[8], &argp9, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
+  if (!SWIG_IsOK(res9)) {
+    SWIG_exception_fail(SWIG_ArgError(res9), "in method '" "predictSensorsMeasurements" "', argument " "9"" of type '" "iDynTree::FreeFloatingAcc &""'"); 
+  }
+  if (!argp9) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "9"" of type '" "iDynTree::FreeFloatingAcc &""'"); 
+  }
+  arg9 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp9);
+  res10 = SWIG_ConvertPtr(argv[9], &argp10, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
+  if (!SWIG_IsOK(res10)) {
+    SWIG_exception_fail(SWIG_ArgError(res10), "in method '" "predictSensorsMeasurements" "', argument " "10"" of type '" "iDynTree::LinkPositions &""'"); 
+  }
+  if (!argp10) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "10"" of type '" "iDynTree::LinkPositions &""'"); 
+  }
+  arg10 = reinterpret_cast< iDynTree::LinkPositions * >(argp10);
+  res11 = SWIG_ConvertPtr(argv[10], &argp11, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res11)) {
+    SWIG_exception_fail(SWIG_ArgError(res11), "in method '" "predictSensorsMeasurements" "', argument " "11"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp11) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "11"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg11 = reinterpret_cast< iDynTree::LinkVelArray * >(argp11);
+  res12 = SWIG_ConvertPtr(argv[11], &argp12, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res12)) {
+    SWIG_exception_fail(SWIG_ArgError(res12), "in method '" "predictSensorsMeasurements" "', argument " "12"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp12) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "12"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg12 = reinterpret_cast< iDynTree::LinkAccArray * >(argp12);
+  res13 = SWIG_ConvertPtr(argv[12], &argp13, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res13)) {
+    SWIG_exception_fail(SWIG_ArgError(res13), "in method '" "predictSensorsMeasurements" "', argument " "13"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+  }
+  if (!argp13) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "13"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+  }
+  arg13 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp13);
+  res14 = SWIG_ConvertPtr(argv[13], &argp14, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques,  0 );
+  if (!SWIG_IsOK(res14)) {
+    SWIG_exception_fail(SWIG_ArgError(res14), "in method '" "predictSensorsMeasurements" "', argument " "14"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+  }
+  if (!argp14) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "14"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+  }
+  arg14 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp14);
+  res15 = SWIG_ConvertPtr(argv[14], &argp15, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
+  if (!SWIG_IsOK(res15)) {
+    SWIG_exception_fail(SWIG_ArgError(res15), "in method '" "predictSensorsMeasurements" "', argument " "15"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+  }
+  if (!argp15) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "15"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+  }
+  arg15 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp15);
+  result = (bool)iDynTree::predictSensorsMeasurements((iDynTree::Model const &)*arg1,(iDynTree::SensorsList const &)*arg2,(iDynTree::Traversal const &)*arg3,(iDynTree::FreeFloatingPos const &)*arg4,(iDynTree::FreeFloatingVel const &)*arg5,(iDynTree::FreeFloatingAcc const &)*arg6,(iDynTree::LinearMotionVector3 const &)*arg7,(iDynTree::LinkWrenches const &)*arg8,*arg9,*arg10,*arg11,*arg12,*arg13,*arg14,*arg15);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_predictSensorsMeasurementsFromRawBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::SensorsList *arg2 = 0 ;
+  iDynTree::Traversal *arg3 = 0 ;
+  iDynTree::LinkVelArray *arg4 = 0 ;
+  iDynTree::LinkAccArray *arg5 = 0 ;
+  iDynTree::LinkInternalWrenches *arg6 = 0 ;
+  iDynTree::SensorsMeasurements *arg7 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("predictSensorsMeasurementsFromRawBuffers",argc,7,7,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Traversal * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "7"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "7"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp7);
+  result = (bool)iDynTree::predictSensorsMeasurementsFromRawBuffers((iDynTree::Model const &)*arg1,(iDynTree::SensorsList const &)*arg2,(iDynTree::Traversal const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::LinkAccArray const &)*arg5,(iDynTree::LinkWrenches const &)*arg6,*arg7);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
+  bool arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("URDFParserOptions_addSensorFramesAsAdditionalFrames_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "URDFParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->addSensorFramesAsAdditionalFrames = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("URDFParserOptions_addSensorFramesAsAdditionalFrames_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_addSensorFramesAsAdditionalFrames_get" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
+  result = (bool) ((arg1)->addSensorFramesAsAdditionalFrames);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_URDFParserOptions_originalFilename_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("URDFParserOptions_originalFilename_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_originalFilename_set" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "URDFParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "URDFParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  if (arg1) (arg1)->originalFilename = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_URDFParserOptions_originalFilename_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::string *result = 0 ;
+  
+  if (!SWIG_check_num_args("URDFParserOptions_originalFilename_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_originalFilename_get" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
+  result = (std::string *) & ((arg1)->originalFilename);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_URDFParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::URDFParserOptions *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_URDFParserOptions",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::URDFParserOptions *)new iDynTree::URDFParserOptions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__URDFParserOptions, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_URDFParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_URDFParserOptions",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_URDFParserOptions" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_modelFromURDF__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::URDFParserOptions arg3 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("modelFromURDF",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  {
+    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__URDFParserOptions,  0 );
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "modelFromURDF" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'"); 
+    }  
+    if (!argp3) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'");
+    } else {
+      arg3 = *(reinterpret_cast< iDynTree::URDFParserOptions * >(argp3));
+    }
+  }
+  result = (bool)iDynTree::modelFromURDF((std::string const &)*arg1,*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_modelFromURDF__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("modelFromURDF",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)iDynTree::modelFromURDF((std::string const &)*arg1,*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_modelFromURDF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_modelFromURDF__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__URDFParserOptions, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_modelFromURDF__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'modelFromURDF'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::modelFromURDF(std::string const &,iDynTree::Model &,iDynTree::URDFParserOptions const)\n"
+    "    iDynTree::modelFromURDF(std::string const &,iDynTree::Model &)\n");
+  return 1;
+}
+
+
+int _wrap_modelFromURDFString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::URDFParserOptions arg3 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("modelFromURDFString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  {
+    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__URDFParserOptions,  0 );
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "modelFromURDFString" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'"); 
+    }  
+    if (!argp3) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'");
+    } else {
+      arg3 = *(reinterpret_cast< iDynTree::URDFParserOptions * >(argp3));
+    }
+  }
+  result = (bool)iDynTree::modelFromURDFString((std::string const &)*arg1,*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_modelFromURDFString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("modelFromURDFString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)iDynTree::modelFromURDFString((std::string const &)*arg1,*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_modelFromURDFString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_modelFromURDFString__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__URDFParserOptions, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_modelFromURDFString__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'modelFromURDFString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::modelFromURDFString(std::string const &,iDynTree::Model &,iDynTree::URDFParserOptions const)\n"
+    "    iDynTree::modelFromURDFString(std::string const &,iDynTree::Model &)\n");
+  return 1;
+}
+
+
+int _wrap_dofsListFromURDF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  std::vector< std::string,std::allocator< std::string > > *arg2 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("dofsListFromURDF",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dofsListFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dofsListFromURDF" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDF" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< std::string,std::allocator< std::string > > * >(argp2);
+  result = (bool)iDynTree::dofsListFromURDF((std::string const &)*arg1,*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_dofsListFromURDFString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  std::vector< std::string,std::allocator< std::string > > *arg2 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("dofsListFromURDFString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dofsListFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dofsListFromURDFString" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDFString" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< std::string,std::allocator< std::string > > * >(argp2);
+  result = (bool)iDynTree::dofsListFromURDFString((std::string const &)*arg1,*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_sensorsFromURDF__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::SensorsList *arg2 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("sensorsFromURDF",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
+  result = (bool)iDynTree::sensorsFromURDF((std::string const &)*arg1,*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_sensorsFromURDF__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("sensorsFromURDF",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "sensorsFromURDF" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  result = (bool)iDynTree::sensorsFromURDF((std::string const &)*arg1,(iDynTree::Model const &)*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_sensorsFromURDF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_sensorsFromURDF__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_sensorsFromURDF__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'sensorsFromURDF'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::sensorsFromURDF(std::string const &,iDynTree::SensorsList &)\n"
+    "    iDynTree::sensorsFromURDF(std::string const &,iDynTree::Model const &,iDynTree::SensorsList &)\n");
+  return 1;
+}
+
+
+int _wrap_sensorsFromURDFString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::SensorsList *arg2 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("sensorsFromURDFString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
+  result = (bool)iDynTree::sensorsFromURDFString((std::string const &)*arg1,*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_sensorsFromURDFString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  std::string *arg1 = 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
+  int res1 = SWIG_OLDOBJ ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("sensorsFromURDFString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
+    if (!SWIG_IsOK(res1)) {
+      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    }
+    arg1 = ptr;
+  }
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "sensorsFromURDFString" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  result = (bool)iDynTree::sensorsFromURDFString((std::string const &)*arg1,(iDynTree::Model const &)*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res1)) delete arg1;
+  return 1;
+}
+
+
+int _wrap_sensorsFromURDFString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_sensorsFromURDFString__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_sensorsFromURDFString__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'sensorsFromURDFString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::sensorsFromURDFString(std::string const &,iDynTree::SensorsList &)\n"
+    "    iDynTree::sensorsFromURDFString(std::string const &,iDynTree::Model const &,iDynTree::SensorsList &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+  bool arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ModelParserOptions_addSensorFramesAsAdditionalFrames_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ModelParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->addSensorFramesAsAdditionalFrames = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelParserOptions_addSensorFramesAsAdditionalFrames_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_addSensorFramesAsAdditionalFrames_get" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
+  result = (bool) ((arg1)->addSensorFramesAsAdditionalFrames);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelParserOptions_originalFilename_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ModelParserOptions_originalFilename_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_originalFilename_set" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  if (arg1) (arg1)->originalFilename = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelParserOptions_originalFilename_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::string *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelParserOptions_originalFilename_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_originalFilename_get" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
+  result = (std::string *) & ((arg1)->originalFilename);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_ModelParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::ModelParserOptions *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_ModelParserOptions",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::ModelParserOptions *)new iDynTree::ModelParserOptions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelParserOptions, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_ModelParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ModelParserOptions",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelParserOptions" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_ModelLoader(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::ModelLoader *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_ModelLoader",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::ModelLoader *)new iDynTree::ModelLoader();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelLoader, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_ModelLoader(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ModelLoader",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelLoader" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelLoader_parsingOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::ModelParserOptions *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelLoader_parsingOptions",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_parsingOptions" "', argument " "1"" of type '" "iDynTree::ModelLoader const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  result = (iDynTree::ModelParserOptions *) &((iDynTree::ModelLoader const *)arg1)->parsingOptions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelLoader_setParsingOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  iDynTree::ModelParserOptions *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ModelLoader_setParsingOptions",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_setParsingOptions" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__ModelParserOptions,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_setParsingOptions" "', argument " "2"" of type '" "iDynTree::ModelParserOptions const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_setParsingOptions" "', argument " "2"" of type '" "iDynTree::ModelParserOptions const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp2);
+  (arg1)->setParsingOptions((iDynTree::ModelParserOptions const &)*arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadModelFromString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadModelFromString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadModelFromString" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromString" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromString((std::string const &)*arg2,(std::string const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadModelFromString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadModelFromString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromString((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadModelFromString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelLoader_loadModelFromString__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelLoader_loadModelFromString__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadModelFromString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelLoader::loadModelFromString(std::string const &,std::string const &)\n"
+    "    iDynTree::ModelLoader::loadModelFromString(std::string const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadModelFromFile",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadModelFromFile" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromFile" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromFile((std::string const &)*arg2,(std::string const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadModelFromFile",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromFile((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelLoader_loadModelFromFile__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelLoader_loadModelFromFile__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadModelFromFile'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelLoader::loadModelFromFile(std::string const &,std::string const &)\n"
+    "    iDynTree::ModelLoader::loadModelFromFile(std::string const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  std::string arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFullModel",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->loadReducedModelFromFullModel((iDynTree::Model const &)*arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFullModel",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadReducedModelFromFullModel((iDynTree::Model const &)*arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromFullModel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadReducedModelFromFullModel'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelLoader::loadReducedModelFromFullModel(iDynTree::Model const &,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
+    "    iDynTree::ModelLoader::loadReducedModelFromFullModel(iDynTree::Model const &,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  std::string arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromString",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->loadReducedModelFromString(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadReducedModelFromString(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelLoader_loadReducedModelFromString__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ModelLoader_loadReducedModelFromString__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadReducedModelFromString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelLoader::loadReducedModelFromString(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
+    "    iDynTree::ModelLoader::loadReducedModelFromString(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  std::string arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFile",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->loadReducedModelFromFile(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFile",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadReducedModelFromFile(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelLoader_loadReducedModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelLoader_loadReducedModelFromFile__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ModelLoader_loadReducedModelFromFile__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadReducedModelFromFile'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelLoader::loadReducedModelFromFile(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
+    "    iDynTree::ModelLoader::loadReducedModelFromFile(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelLoader_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Model *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelLoader_model",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_model" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  result = (iDynTree::Model *) &(arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelLoader_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::SensorsList *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelLoader_sensors",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_sensors" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  result = (iDynTree::SensorsList *) &(arg1)->sensors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelLoader_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelLoader_isValid",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_isValid" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  result = (bool)(arg1)->isValid();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_UnknownWrenchContact__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_UnknownWrenchContact__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContactType arg1 ;
+  iDynTree::Position *arg2 = 0 ;
+  iDynTree::Direction *arg3 = 0 ;
+  iDynTree::Wrench *arg4 = 0 ;
+  unsigned long *arg5 = 0 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  unsigned long temp5 ;
+  unsigned long val5 ;
+  int ecode5 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,5,5,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
+  } 
+  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Wrench,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Wrench * >(argp4);
+  ecode5 = SWIG_AsVal_unsigned_SS_long(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "new_UnknownWrenchContact" "', argument " "5"" of type '" "unsigned long""'");
+  } 
+  temp5 = static_cast< unsigned long >(val5);
+  arg5 = &temp5;
+  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3,(iDynTree::Wrench const &)*arg4,(unsigned long const &)*arg5);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_UnknownWrenchContact__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContactType arg1 ;
+  iDynTree::Position *arg2 = 0 ;
+  iDynTree::Direction *arg3 = 0 ;
+  iDynTree::Wrench *arg4 = 0 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
+  } 
+  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Wrench,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Wrench * >(argp4);
+  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3,(iDynTree::Wrench const &)*arg4);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_UnknownWrenchContact__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContactType arg1 ;
+  iDynTree::Position *arg2 = 0 ;
+  iDynTree::Direction *arg3 = 0 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
+  } 
+  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
+  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_UnknownWrenchContact__SWIG_4(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContactType arg1 ;
+  iDynTree::Position *arg2 = 0 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
+  } 
+  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_UnknownWrenchContact(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_UnknownWrenchContact__SWIG_0(resc,resv,argc,argv);
+  }
+  if (argc == 2) {
+    int _v;
+    {
+      int res = SWIG_AsVal_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_new_UnknownWrenchContact__SWIG_4(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    {
+      int res = SWIG_AsVal_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_new_UnknownWrenchContact__SWIG_3(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    {
+      int res = SWIG_AsVal_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Wrench, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_new_UnknownWrenchContact__SWIG_2(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 5) {
+    int _v;
+    {
+      int res = SWIG_AsVal_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Wrench, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            {
+              int res = SWIG_AsVal_unsigned_SS_long(argv[4], NULL);
+              _v = SWIG_CheckState(res);
+            }
+            if (_v) {
+              return _wrap_new_UnknownWrenchContact__SWIG_1(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_UnknownWrenchContact'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact()\n"
+    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &,iDynTree::Direction const &,iDynTree::Wrench const &,unsigned long const &)\n"
+    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &,iDynTree::Direction const &,iDynTree::Wrench const &)\n"
+    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &,iDynTree::Direction const &)\n"
+    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &)\n");
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_unknownType_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  iDynTree::UnknownWrenchContactType arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_unknownType_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_unknownType_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "UnknownWrenchContact_unknownType_set" "', argument " "2"" of type '" "iDynTree::UnknownWrenchContactType""'");
+  } 
+  arg2 = static_cast< iDynTree::UnknownWrenchContactType >(val2);
+  if (arg1) (arg1)->unknownType = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_unknownType_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContactType result;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_unknownType_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_unknownType_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  result = (iDynTree::UnknownWrenchContactType) ((arg1)->unknownType);
+  _out = SWIG_From_int(static_cast< int >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_contactPoint_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  iDynTree::Position *arg2 = (iDynTree::Position *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_contactPoint_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactPoint_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Position, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "UnknownWrenchContact_contactPoint_set" "', argument " "2"" of type '" "iDynTree::Position *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  if (arg1) (arg1)->contactPoint = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_contactPoint_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Position *result = 0 ;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_contactPoint_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactPoint_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  result = (iDynTree::Position *)& ((arg1)->contactPoint);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Position, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_forceDirection_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  iDynTree::Direction *arg2 = (iDynTree::Direction *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_forceDirection_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_forceDirection_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Direction, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "UnknownWrenchContact_forceDirection_set" "', argument " "2"" of type '" "iDynTree::Direction *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Direction * >(argp2);
+  if (arg1) (arg1)->forceDirection = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_forceDirection_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Direction *result = 0 ;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_forceDirection_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_forceDirection_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  result = (iDynTree::Direction *)& ((arg1)->forceDirection);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Direction, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_knownWrench_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  iDynTree::Wrench *arg2 = (iDynTree::Wrench *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_knownWrench_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_knownWrench_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "UnknownWrenchContact_knownWrench_set" "', argument " "2"" of type '" "iDynTree::Wrench *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Wrench * >(argp2);
+  if (arg1) (arg1)->knownWrench = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_knownWrench_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Wrench *result = 0 ;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_knownWrench_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_knownWrench_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  result = (iDynTree::Wrench *)& ((arg1)->knownWrench);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_contactId_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  unsigned long arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned long val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_contactId_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactId_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_long(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "UnknownWrenchContact_contactId_set" "', argument " "2"" of type '" "unsigned long""'");
+  } 
+  arg2 = static_cast< unsigned long >(val2);
+  if (arg1) (arg1)->contactId = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_UnknownWrenchContact_contactId_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  unsigned long result;
+  
+  if (!SWIG_check_num_args("UnknownWrenchContact_contactId_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactId_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  result = (unsigned long) ((arg1)->contactId);
+  _out = SWIG_From_unsigned_SS_long(static_cast< unsigned long >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_UnknownWrenchContact(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_UnknownWrenchContact",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkUnknownWrenchContacts__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  unsigned int arg1 ;
+  unsigned int val1 ;
+  int ecode1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkUnknownWrenchContacts *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_LinkUnknownWrenchContacts",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "unsigned int""'");
+  } 
+  arg1 = static_cast< unsigned int >(val1);
+  result = (iDynTree::LinkUnknownWrenchContacts *)new iDynTree::LinkUnknownWrenchContacts(arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkUnknownWrenchContacts__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::LinkUnknownWrenchContacts *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_LinkUnknownWrenchContacts",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::LinkUnknownWrenchContacts *)new iDynTree::LinkUnknownWrenchContacts();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkUnknownWrenchContacts__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkUnknownWrenchContacts *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_LinkUnknownWrenchContacts",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (iDynTree::LinkUnknownWrenchContacts *)new iDynTree::LinkUnknownWrenchContacts((iDynTree::Model const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_LinkUnknownWrenchContacts(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_LinkUnknownWrenchContacts__SWIG_1(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_LinkUnknownWrenchContacts__SWIG_2(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    {
+      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      return _wrap_new_LinkUnknownWrenchContacts__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkUnknownWrenchContacts'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkUnknownWrenchContacts::LinkUnknownWrenchContacts(unsigned int)\n"
+    "    iDynTree::LinkUnknownWrenchContacts::LinkUnknownWrenchContacts()\n"
+    "    iDynTree::LinkUnknownWrenchContacts::LinkUnknownWrenchContacts(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_clear(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_clear",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_clear" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  (arg1)->clear();
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  unsigned int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  unsigned int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_resize",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "2"" of type '" "unsigned int""'");
+  } 
+  arg2 = static_cast< unsigned int >(val2);
+  (arg1)->resize(arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_resize",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  (arg1)->resize((iDynTree::Model const &)*arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_LinkUnknownWrenchContacts_resize__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_LinkUnknownWrenchContacts_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkUnknownWrenchContacts_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkUnknownWrenchContacts::resize(unsigned int)\n"
+    "    iDynTree::LinkUnknownWrenchContacts::resize(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_getNrOfContactsForLink",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_getNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_getNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = ((iDynTree::LinkUnknownWrenchContacts const *)arg1)->getNrOfContactsForLink(arg2);
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_setNrOfContactsForLink",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_setNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_setNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_setNrOfContactsForLink" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  (arg1)->setNrOfContactsForLink(arg2,arg3);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_addNewContactForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::UnknownWrenchContact *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_addNewContactForLink",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__UnknownWrenchContact,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "3"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "3"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp3);
+  (arg1)->addNewContactForLink(arg2,(iDynTree::UnknownWrenchContact const &)*arg3);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_addNewContactInFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::FrameIndex arg3 ;
+  iDynTree::UnknownWrenchContact *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_addNewContactInFrame",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::FrameIndex >(val3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__UnknownWrenchContact,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "4"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "4"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp4);
+  result = (bool)(arg1)->addNewContactInFrame((iDynTree::Model const &)*arg2,arg3,(iDynTree::UnknownWrenchContact const &)*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::FrameIndex arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::FrameIndex >(val3);
+  result = (bool)(arg1)->addNewUnknownFullWrenchInFrameOrigin((iDynTree::Model const &)*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_contactWrench",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  result = (iDynTree::UnknownWrenchContact *) &(arg1)->contactWrench(arg2,arg3);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  iDynTree::UnknownWrenchContact *result = 0 ;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_contactWrench",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  result = (iDynTree::UnknownWrenchContact *) &((iDynTree::LinkUnknownWrenchContacts const *)arg1)->contactWrench(arg2,arg3);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_contactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkUnknownWrenchContacts_contactWrench'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::LinkUnknownWrenchContacts::contactWrench(iDynTree::LinkIndex const,size_t const)\n"
+    "    iDynTree::LinkUnknownWrenchContacts::contactWrench(iDynTree::LinkIndex const,size_t const) const\n");
+  return 1;
+}
+
+
+int _wrap_LinkUnknownWrenchContacts_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  std::string result;
+  
+  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_toString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_toString" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = ((iDynTree::LinkUnknownWrenchContacts const *)arg1)->toString((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_LinkUnknownWrenchContacts(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_LinkUnknownWrenchContacts",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_estimateExternalWrenchesBuffers__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::estimateExternalWrenchesBuffers *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_estimateExternalWrenchesBuffers",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::estimateExternalWrenchesBuffers *)new iDynTree::estimateExternalWrenchesBuffers();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_estimateExternalWrenchesBuffers__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SubModelDecomposition *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::estimateExternalWrenchesBuffers *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_estimateExternalWrenchesBuffers",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp1);
+  result = (iDynTree::estimateExternalWrenchesBuffers *)new iDynTree::estimateExternalWrenchesBuffers((iDynTree::SubModelDecomposition const &)*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_estimateExternalWrenchesBuffers__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  size_t arg1 ;
+  size_t arg2 ;
+  size_t val1 ;
+  int ecode1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  iDynTree::estimateExternalWrenchesBuffers *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_estimateExternalWrenchesBuffers",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "size_t""'");
+  } 
+  arg1 = static_cast< size_t >(val1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  result = (iDynTree::estimateExternalWrenchesBuffers *)new iDynTree::estimateExternalWrenchesBuffers(arg1,arg2);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_estimateExternalWrenchesBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 0) {
+    return _wrap_new_estimateExternalWrenchesBuffers__SWIG_0(resc,resv,argc,argv);
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SubModelDecomposition, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_new_estimateExternalWrenchesBuffers__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    {
+      int res = SWIG_AsVal_size_t(argv[0], NULL);
+      _v = SWIG_CheckState(res);
+    }
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_new_estimateExternalWrenchesBuffers__SWIG_2(resc,resv,argc,argv);
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_estimateExternalWrenchesBuffers'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::estimateExternalWrenchesBuffers::estimateExternalWrenchesBuffers()\n"
+    "    iDynTree::estimateExternalWrenchesBuffers::estimateExternalWrenchesBuffers(iDynTree::SubModelDecomposition const &)\n"
+    "    iDynTree::estimateExternalWrenchesBuffers::estimateExternalWrenchesBuffers(size_t const,size_t const)\n");
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  iDynTree::SubModelDecomposition *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_resize",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp2);
+  (arg1)->resize((iDynTree::SubModelDecomposition const &)*arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  size_t arg2 ;
+  size_t arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_resize",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "3"" of type '" "size_t""'");
+  } 
+  arg3 = static_cast< size_t >(val3);
+  (arg1)->resize(arg2,arg3);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SubModelDecomposition, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_estimateExternalWrenchesBuffers_resize__SWIG_0(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_estimateExternalWrenchesBuffers_resize__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'estimateExternalWrenchesBuffers_resize'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::estimateExternalWrenchesBuffers::resize(iDynTree::SubModelDecomposition const &)\n"
+    "    iDynTree::estimateExternalWrenchesBuffers::resize(size_t const,size_t const)\n");
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_getNrOfSubModels",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_getNrOfSubModels" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = ((iDynTree::estimateExternalWrenchesBuffers const *)arg1)->getNrOfSubModels();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_getNrOfLinks",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = ((iDynTree::estimateExternalWrenchesBuffers const *)arg1)->getNrOfLinks();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  iDynTree::SubModelDecomposition *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_isConsistent",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_isConsistent" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp2);
+  result = (bool)((iDynTree::estimateExternalWrenchesBuffers const *)arg1)->isConsistent((iDynTree::SubModelDecomposition const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_A_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *arg2 = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_A_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_A_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_A_set" "', argument " "2"" of type '" "std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > * >(argp2);
+  if (arg1) (arg1)->A = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_A_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *result = 0 ;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_A_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_A_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *)& ((arg1)->A);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_x_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *arg2 = (std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_x_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_x_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_x_set" "', argument " "2"" of type '" "std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > * >(argp2);
+  if (arg1) (arg1)->x = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_x_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *result = 0 ;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_x_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_x_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = (std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *)& ((arg1)->x);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_b_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *arg2 = (std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_b_set" "', argument " "2"" of type '" "std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > * >(argp2);
+  if (arg1) (arg1)->b = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_b_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *result = 0 ;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = (std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *)& ((arg1)->b);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_pinvA_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *arg2 = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_pinvA_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_pinvA_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_pinvA_set" "', argument " "2"" of type '" "std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *""'"); 
+  }
+  arg2 = reinterpret_cast< std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > * >(argp2);
+  if (arg1) (arg1)->pinvA = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_pinvA_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *result = 0 ;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_pinvA_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_pinvA_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *)& ((arg1)->pinvA);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  iDynTree::LinkWrenches *arg2 = (iDynTree::LinkWrenches *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_contacts_subtree_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_contacts_subtree_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_b_contacts_subtree_set" "', argument " "2"" of type '" "iDynTree::LinkWrenches *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkWrenches * >(argp2);
+  if (arg1) (arg1)->b_contacts_subtree = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkWrenches *result = 0 ;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_contacts_subtree_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_contacts_subtree_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = (iDynTree::LinkWrenches *)& ((arg1)->b_contacts_subtree);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  iDynTree::LinkPositions *arg2 = (iDynTree::LinkPositions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_subModelBase_H_link_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_subModelBase_H_link_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_subModelBase_H_link_set" "', argument " "2"" of type '" "iDynTree::LinkPositions *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkPositions * >(argp2);
+  if (arg1) (arg1)->subModelBase_H_link = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::LinkPositions *result = 0 ;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_subModelBase_H_link_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_subModelBase_H_link_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  result = (iDynTree::LinkPositions *)& ((arg1)->subModelBase_H_link);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_estimateExternalWrenchesBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_estimateExternalWrenchesBuffers",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenchesWithoutInternalFT(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::LinkUnknownWrenchContacts *arg3 = 0 ;
+  iDynTree::JointPosDoubleArray *arg4 = 0 ;
+  iDynTree::LinkVelArray *arg5 = 0 ;
+  iDynTree::LinkAccArray *arg6 = 0 ;
+  iDynTree::estimateExternalWrenchesBuffers *arg7 = 0 ;
+  iDynTree::LinkContactWrenches *arg8 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
+  void *argp8 = 0 ;
+  int res8 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenchesWithoutInternalFT",argc,8,8,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "3"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "3"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "6"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "6"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "7"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "7"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "8"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "8"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp8);
+  result = (bool)iDynTree::estimateExternalWrenchesWithoutInternalFT((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::LinkUnknownWrenchContacts const &)*arg3,(iDynTree::JointPosDoubleArray const &)*arg4,(iDynTree::LinkVelArray const &)*arg5,(iDynTree::LinkAccArray const &)*arg6,*arg7,*arg8);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_estimateExternalWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::SubModelDecomposition *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
+  iDynTree::LinkUnknownWrenchContacts *arg4 = 0 ;
+  iDynTree::JointPosDoubleArray *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  iDynTree::LinkAccArray *arg7 = 0 ;
+  iDynTree::SensorsMeasurements *arg8 = 0 ;
+  iDynTree::estimateExternalWrenchesBuffers *arg9 = 0 ;
+  iDynTree::LinkContactWrenches *arg10 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 ;
+  int res7 = 0 ;
+  void *argp8 ;
+  int res8 = 0 ;
+  void *argp9 = 0 ;
+  int res9 = 0 ;
+  void *argp10 = 0 ;
+  int res10 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("estimateExternalWrenches",argc,10,10,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenches" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateExternalWrenches" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "estimateExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "estimateExternalWrenches" "', argument " "5"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "5"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "estimateExternalWrenches" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "estimateExternalWrenches" "', argument " "7"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "7"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "estimateExternalWrenches" "', argument " "8"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "8"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp8);
+  res9 = SWIG_ConvertPtr(argv[8], &argp9, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers,  0 );
+  if (!SWIG_IsOK(res9)) {
+    SWIG_exception_fail(SWIG_ArgError(res9), "in method '" "estimateExternalWrenches" "', argument " "9"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
+  }
+  if (!argp9) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "9"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
+  }
+  arg9 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp9);
+  res10 = SWIG_ConvertPtr(argv[9], &argp10, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
+  if (!SWIG_IsOK(res10)) {
+    SWIG_exception_fail(SWIG_ArgError(res10), "in method '" "estimateExternalWrenches" "', argument " "10"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  if (!argp10) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "10"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  arg10 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp10);
+  result = (bool)iDynTree::estimateExternalWrenches((iDynTree::Model const &)*arg1,(iDynTree::SubModelDecomposition const &)*arg2,(iDynTree::SensorsList const &)*arg3,(iDynTree::LinkUnknownWrenchContacts const &)*arg4,(iDynTree::JointPosDoubleArray const &)*arg5,(iDynTree::LinkVelArray const &)*arg6,(iDynTree::LinkAccArray const &)*arg7,(iDynTree::SensorsMeasurements const &)*arg8,*arg9,*arg10);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_dynamicsEstimationForwardVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::Vector3 *arg3 = 0 ;
+  iDynTree::Vector3 *arg4 = 0 ;
+  iDynTree::Vector3 *arg5 = 0 ;
+  iDynTree::JointPosDoubleArray *arg6 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg7 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg8 = 0 ;
+  iDynTree::LinkVelArray *arg9 = 0 ;
+  iDynTree::LinkAccArray *arg10 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 ;
+  int res7 = 0 ;
+  void *argp8 ;
+  int res8 = 0 ;
+  void *argp9 = 0 ;
+  int res9 = 0 ;
+  void *argp10 = 0 ;
+  int res10 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("dynamicsEstimationForwardVelAccKinematics",argc,10,10,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Vector3 * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Vector3 * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "8"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "8"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp8);
+  res9 = SWIG_ConvertPtr(argv[8], &argp9, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res9)) {
+    SWIG_exception_fail(SWIG_ArgError(res9), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "9"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp9) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "9"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg9 = reinterpret_cast< iDynTree::LinkVelArray * >(argp9);
+  res10 = SWIG_ConvertPtr(argv[9], &argp10, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res10)) {
+    SWIG_exception_fail(SWIG_ArgError(res10), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "10"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  if (!argp10) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "10"" of type '" "iDynTree::LinkAccArray &""'"); 
+  }
+  arg10 = reinterpret_cast< iDynTree::LinkAccArray * >(argp10);
+  result = (bool)iDynTree::dynamicsEstimationForwardVelAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::VectorFixSize< 3 > const &)*arg3,(iDynTree::VectorFixSize< 3 > const &)*arg4,(iDynTree::VectorFixSize< 3 > const &)*arg5,(iDynTree::JointPosDoubleArray const &)*arg6,(iDynTree::JointDOFsDoubleArray const &)*arg7,(iDynTree::JointDOFsDoubleArray const &)*arg8,*arg9,*arg10);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_dynamicsEstimationForwardVelKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::Traversal *arg2 = 0 ;
+  iDynTree::Vector3 *arg3 = 0 ;
+  iDynTree::JointPosDoubleArray *arg4 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
+  iDynTree::LinkVelArray *arg6 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 = 0 ;
+  int res6 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("dynamicsEstimationForwardVelKinematics",argc,6,6,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Vector3 * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setName" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setName" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+  arg4 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
-  result = (bool)(arg1)->setName((std::string const &)*arg2);
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
+  result = (bool)iDynTree::dynamicsEstimationForwardVelKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::VectorFixSize< 3 > const &)*arg3,(iDynTree::JointPosDoubleArray const &)*arg4,(iDynTree::JointDOFsDoubleArray const &)*arg5,*arg6);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  iDynTree::Transform *arg2 = 0 ;
-  void *argp1 = 0 ;
+int _wrap_computeLinkNetWrenchesWithoutGravity(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::LinkVelArray *arg2 = 0 ;
+  iDynTree::LinkAccArray *arg3 = 0 ;
+  iDynTree::LinkNetTotalWrenchesWithoutGravity *arg4 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setLinkSensorTransform",argc,2,2,0)) {
+  if (!SWIG_check_num_args("computeLinkNetWrenchesWithoutGravity",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform" "', argument " "2"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkVelArray const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Transform * >(argp2);
-  result = (bool)(arg1)->setLinkSensorTransform((iDynTree::Transform const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::LinkVelArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "3"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "3"" of type '" "iDynTree::LinkAccArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::LinkAccArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "4"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "4"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkNetTotalWrenchesWithoutGravity * >(argp4);
+  result = (bool)iDynTree::computeLinkNetWrenchesWithoutGravity((iDynTree::Model const &)*arg1,(iDynTree::LinkVelArray const &)*arg2,(iDynTree::LinkAccArray const &)*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -66550,72 +72615,76 @@ int _wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(int resc, mxA
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_setParentLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  std::string *arg2 = 0 ;
-  void *argp1 = 0 ;
+int _wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = 0 ;
+  iDynTree::LinkUnknownWrenchContacts *arg2 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
+  iDynTree::LinkContactWrenches *arg4 = 0 ;
+  void *argp1 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setParentLink",argc,2,2,0)) {
+  if (!SWIG_check_num_args("estimateLinkContactWrenchesFromLinkNetExternalWrenches",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLink" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setParentLink" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
   }
-  result = (bool)(arg1)->setParentLink((std::string const &)*arg2);
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp4);
+  result = (bool)iDynTree::estimateLinkContactWrenchesFromLinkNetExternalWrenches((iDynTree::Model const &)*arg1,(iDynTree::LinkUnknownWrenchContacts const &)*arg2,(iDynTree::LinkWrenches const &)*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  iDynTree::LinkIndex *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  iDynTree::LinkIndex temp2 ;
-  int val2 ;
-  int ecode2 = 0 ;
+int _wrap_new_ExtWrenchesAndJointTorquesEstimator(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  bool result;
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *result = 0 ;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setParentLinkIndex",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_ExtWrenchesAndJointTorquesEstimator",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLinkIndex" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ThreeAxisForceTorqueContactSensor_setParentLinkIndex" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  temp2 = static_cast< iDynTree::LinkIndex >(val2);
-  arg2 = &temp2;
-  result = (bool)(arg1)->setParentLinkIndex((iDynTree::LinkIndex const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  (void)argv;
+  result = (iDynTree::ExtWrenchesAndJointTorquesEstimator *)new iDynTree::ExtWrenchesAndJointTorquesEstimator();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66623,23 +72692,26 @@ int _wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(int resc, mxArray
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_getName(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+int _wrap_delete_ExtWrenchesAndJointTorquesEstimator(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  std::string result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getName",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ExtWrenchesAndJointTorquesEstimator",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getName" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ExtWrenchesAndJointTorquesEstimator" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getName();
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66647,23 +72719,45 @@ int _wrap_ThreeAxisForceTorqueContactSensor_getName(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_getSensorType(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
-  iDynTree::SensorType result;
+  bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getSensorType",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_setModelAndSensors",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getSensorType" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = (iDynTree::SensorType)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getSensorType();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  result = (bool)(arg1)->setModelAndSensors((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66671,23 +72765,43 @@ int _wrap_ThreeAxisForceTorqueContactSensor_getSensorType(int resc, mxArray *res
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_getParentLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  std::string arg2 ;
+  std::string arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  std::string result;
+  bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getParentLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getParentLink" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getParentLink();
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "3"" of type '" "std::string const""'"); 
+    }
+    arg3 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->loadModelAndSensorsFromFile(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66695,23 +72809,33 @@ int _wrap_ThreeAxisForceTorqueContactSensor_getParentLink(int resc, mxArray *res
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  std::string arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkIndex result;
+  bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getParentLinkIndex",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getParentLinkIndex" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = (iDynTree::LinkIndex)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getParentLinkIndex();
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->loadModelAndSensorsFromFile(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66719,164 +72843,219 @@ int _wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(int resc, mxArray
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_getLinkSensorTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::Transform result;
-  
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getLinkSensorTransform",argc,1,1,0)) {
-    SWIG_fail;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_1(resc,resv,argc,argv);
+      }
+    }
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getLinkSensorTransform" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getLinkSensorTransform();
-  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFile(std::string const,std::string const)\n"
+    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFile(std::string const)\n");
   return 1;
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  std::string arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_isValid",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_isValid" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = (bool)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->isValid();
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_ThreeAxisForceTorqueContactSensor_clone(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::Sensor *result = 0 ;
-  
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_clone",argc,1,1,0)) {
-    SWIG_fail;
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_clone" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = (iDynTree::Sensor *)((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->clone();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Sensor, 0 |  0 );
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->loadModelAndSensorsFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_updateIndices(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_updateIndices",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_updateIndices" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_updateIndices" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_updateIndices" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg3 = ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)(arg1)->updateIndices((iDynTree::Model const &)*arg2);
+  result = (bool)(arg1)->loadModelAndSensorsFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_setLoadCellLocations(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_setLoadCellLocations",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_setLoadCellLocations" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_setLoadCellLocations" "', argument " "2"" of type '" "std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > &""'"); 
+int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_setLoadCellLocations" "', argument " "2"" of type '" "std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > &""'"); 
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
   }
-  arg2 = reinterpret_cast< std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > * >(argp2);
-  (arg1)->setLoadCellLocations(*arg2);
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
+    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
   return 1;
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  SwigValueWrapper< std::vector< iDynTree::Position,std::allocator< iDynTree::Position > > > result;
+  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_getLoadCellLocations",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_model",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_getLoadCellLocations" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_model" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->getLoadCellLocations();
-  _out = SWIG_NewPointerObj((new std::vector< iDynTree::Position,std::allocator< iDynTree::Position > >(static_cast< const std::vector< iDynTree::Position,std::allocator< iDynTree::Position > >& >(result))), SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  result = (iDynTree::Model *) &((iDynTree::ExtWrenchesAndJointTorquesEstimator const *)arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66884,34 +73063,23 @@ int _wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(int resc, mxArr
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Vector3 result;
+  iDynTree::SensorsList *result = 0 ;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_sensors",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_sensors" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->computeThreeAxisForceTorqueFromLoadCellMeasurements(*arg2);
-  _out = SWIG_NewPointerObj((new iDynTree::Vector3(static_cast< const iDynTree::Vector3& >(result))), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  result = (iDynTree::SensorsList *) &((iDynTree::ExtWrenchesAndJointTorquesEstimator const *)arg1)->sensors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66919,34 +73087,23 @@ int _wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadC
 }
 
 
-int _wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ThreeAxisForceTorqueContactSensor *arg1 = (iDynTree::ThreeAxisForceTorqueContactSensor *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_submodels(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Position result;
+  iDynTree::SubModelDecomposition *result = 0 ;
   
-  if (!SWIG_check_num_args("ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_submodels",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements" "', argument " "1"" of type '" "iDynTree::ThreeAxisForceTorqueContactSensor const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ThreeAxisForceTorqueContactSensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_submodels" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  result = ((iDynTree::ThreeAxisForceTorqueContactSensor const *)arg1)->computeCenterOfPressureFromLoadCellMeasurements(*arg2);
-  _out = SWIG_NewPointerObj((new iDynTree::Position(static_cast< const iDynTree::Position& >(result))), SWIGTYPE_p_iDynTree__Position, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  result = (iDynTree::SubModelDecomposition *) &((iDynTree::ExtWrenchesAndJointTorquesEstimator const *)arg1)->submodels();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SubModelDecomposition, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -66954,23 +73111,16 @@ int _wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellM
 }
 
 
-int _wrap_predictSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::SensorsList *arg2 = 0 ;
-  iDynTree::Traversal *arg3 = 0 ;
-  iDynTree::FreeFloatingPos *arg4 = 0 ;
-  iDynTree::FreeFloatingVel *arg5 = 0 ;
-  iDynTree::FreeFloatingAcc *arg6 = 0 ;
-  iDynTree::LinAcceleration *arg7 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg8 = 0 ;
-  iDynTree::FreeFloatingAcc *arg9 = 0 ;
-  iDynTree::LinkPositions *arg10 = 0 ;
-  iDynTree::LinkVelArray *arg11 = 0 ;
-  iDynTree::LinkAccArray *arg12 = 0 ;
-  iDynTree::LinkInternalWrenches *arg13 = 0 ;
-  iDynTree::FreeFloatingGeneralizedTorques *arg14 = 0 ;
-  iDynTree::SensorsMeasurements *arg15 = 0 ;
-  void *argp1 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg4 = 0 ;
+  iDynTree::FrameIndex *arg5 = 0 ;
+  iDynTree::Vector3 *arg6 = 0 ;
+  iDynTree::Vector3 *arg7 = 0 ;
+  iDynTree::Vector3 *arg8 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
@@ -66978,155 +73128,81 @@ int _wrap_predictSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArra
   int res3 = 0 ;
   void *argp4 ;
   int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
+  iDynTree::FrameIndex temp5 ;
+  int val5 ;
+  int ecode5 = 0 ;
   void *argp6 ;
   int res6 = 0 ;
   void *argp7 ;
   int res7 = 0 ;
   void *argp8 ;
   int res8 = 0 ;
-  void *argp9 = 0 ;
-  int res9 = 0 ;
-  void *argp10 = 0 ;
-  int res10 = 0 ;
-  void *argp11 = 0 ;
-  int res11 = 0 ;
-  void *argp12 = 0 ;
-  int res12 = 0 ;
-  void *argp13 = 0 ;
-  int res13 = 0 ;
-  void *argp14 = 0 ;
-  int res14 = 0 ;
-  void *argp15 = 0 ;
-  int res15 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("predictSensorsMeasurements",argc,15,15,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "predictSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "predictSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "predictSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Traversal * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__FreeFloatingPos,  0 );
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "predictSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::FreeFloatingPos const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::FreeFloatingPos * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__FreeFloatingVel,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "predictSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::FreeFloatingVel const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::FreeFloatingVel * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
+  arg4 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "5"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  temp5 = static_cast< iDynTree::FrameIndex >(val5);
+  arg5 = &temp5;
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "predictSensorsMeasurements" "', argument " "6"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "6"" of type '" "iDynTree::FreeFloatingAcc const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinearMotionVector3,  0 );
+  arg6 = reinterpret_cast< iDynTree::Vector3 * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
   if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "predictSensorsMeasurements" "', argument " "7"" of type '" "iDynTree::LinAcceleration const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "7"" of type '" "iDynTree::Vector3 const &""'"); 
   }
   if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "7"" of type '" "iDynTree::LinAcceleration const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "7"" of type '" "iDynTree::Vector3 const &""'"); 
   }
-  arg7 = reinterpret_cast< iDynTree::LinAcceleration * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  arg7 = reinterpret_cast< iDynTree::Vector3 * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
   if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "predictSensorsMeasurements" "', argument " "8"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "8"" of type '" "iDynTree::Vector3 const &""'"); 
   }
   if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "8"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
-  }
-  arg8 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp8);
-  res9 = SWIG_ConvertPtr(argv[8], &argp9, SWIGTYPE_p_iDynTree__FreeFloatingAcc,  0 );
-  if (!SWIG_IsOK(res9)) {
-    SWIG_exception_fail(SWIG_ArgError(res9), "in method '" "predictSensorsMeasurements" "', argument " "9"" of type '" "iDynTree::FreeFloatingAcc &""'"); 
-  }
-  if (!argp9) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "9"" of type '" "iDynTree::FreeFloatingAcc &""'"); 
-  }
-  arg9 = reinterpret_cast< iDynTree::FreeFloatingAcc * >(argp9);
-  res10 = SWIG_ConvertPtr(argv[9], &argp10, SWIGTYPE_p_iDynTree__LinkPositions,  0 );
-  if (!SWIG_IsOK(res10)) {
-    SWIG_exception_fail(SWIG_ArgError(res10), "in method '" "predictSensorsMeasurements" "', argument " "10"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  if (!argp10) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "10"" of type '" "iDynTree::LinkPositions &""'"); 
-  }
-  arg10 = reinterpret_cast< iDynTree::LinkPositions * >(argp10);
-  res11 = SWIG_ConvertPtr(argv[10], &argp11, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res11)) {
-    SWIG_exception_fail(SWIG_ArgError(res11), "in method '" "predictSensorsMeasurements" "', argument " "11"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp11) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "11"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg11 = reinterpret_cast< iDynTree::LinkVelArray * >(argp11);
-  res12 = SWIG_ConvertPtr(argv[11], &argp12, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res12)) {
-    SWIG_exception_fail(SWIG_ArgError(res12), "in method '" "predictSensorsMeasurements" "', argument " "12"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp12) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "12"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  arg12 = reinterpret_cast< iDynTree::LinkAccArray * >(argp12);
-  res13 = SWIG_ConvertPtr(argv[12], &argp13, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res13)) {
-    SWIG_exception_fail(SWIG_ArgError(res13), "in method '" "predictSensorsMeasurements" "', argument " "13"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
-  }
-  if (!argp13) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "13"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
-  }
-  arg13 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp13);
-  res14 = SWIG_ConvertPtr(argv[13], &argp14, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques,  0 );
-  if (!SWIG_IsOK(res14)) {
-    SWIG_exception_fail(SWIG_ArgError(res14), "in method '" "predictSensorsMeasurements" "', argument " "14"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
-  }
-  if (!argp14) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "14"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
-  }
-  arg14 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp14);
-  res15 = SWIG_ConvertPtr(argv[14], &argp15, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
-  if (!SWIG_IsOK(res15)) {
-    SWIG_exception_fail(SWIG_ArgError(res15), "in method '" "predictSensorsMeasurements" "', argument " "15"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  if (!argp15) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurements" "', argument " "15"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "8"" of type '" "iDynTree::Vector3 const &""'"); 
   }
-  arg15 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp15);
-  result = (bool)iDynTree::predictSensorsMeasurements((iDynTree::Model const &)*arg1,(iDynTree::SensorsList const &)*arg2,(iDynTree::Traversal const &)*arg3,(iDynTree::FreeFloatingPos const &)*arg4,(iDynTree::FreeFloatingVel const &)*arg5,(iDynTree::FreeFloatingAcc const &)*arg6,(iDynTree::LinearMotionVector3 const &)*arg7,(iDynTree::LinkWrenches const &)*arg8,*arg9,*arg10,*arg11,*arg12,*arg13,*arg14,*arg15);
+  arg8 = reinterpret_cast< iDynTree::Vector3 * >(argp8);
+  result = (bool)(arg1)->updateKinematicsFromFloatingBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::JointDOFsDoubleArray const &)*arg4,(iDynTree::FrameIndex const &)*arg5,(iDynTree::Vector3 const &)*arg6,(iDynTree::Vector3 const &)*arg7,(iDynTree::Vector3 const &)*arg8);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -67135,15 +73211,14 @@ int _wrap_predictSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_predictSensorsMeasurementsFromRawBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::SensorsList *arg2 = 0 ;
-  iDynTree::Traversal *arg3 = 0 ;
-  iDynTree::LinkVelArray *arg4 = 0 ;
-  iDynTree::LinkAccArray *arg5 = 0 ;
-  iDynTree::LinkInternalWrenches *arg6 = 0 ;
-  iDynTree::SensorsMeasurements *arg7 = 0 ;
-  void *argp1 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg4 = 0 ;
+  iDynTree::FrameIndex *arg5 = 0 ;
+  iDynTree::Vector3 *arg6 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
@@ -67151,210 +73226,62 @@ int _wrap_predictSensorsMeasurementsFromRawBuffers(int resc, mxArray *resv[], in
   int res3 = 0 ;
   void *argp4 ;
   int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
+  iDynTree::FrameIndex temp5 ;
+  int val5 ;
+  int ecode5 = 0 ;
   void *argp6 ;
   int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("predictSensorsMeasurementsFromRawBuffers",argc,7,7,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "2"" of type '" "iDynTree::SensorsList const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "3"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Traversal * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "4"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkVelArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "5"" of type '" "iDynTree::LinkAccArray const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkAccArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches const &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "7"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "predictSensorsMeasurementsFromRawBuffers" "', argument " "7"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp7);
-  result = (bool)iDynTree::predictSensorsMeasurementsFromRawBuffers((iDynTree::Model const &)*arg1,(iDynTree::SensorsList const &)*arg2,(iDynTree::Traversal const &)*arg3,(iDynTree::LinkVelArray const &)*arg4,(iDynTree::LinkAccArray const &)*arg5,(iDynTree::LinkWrenches const &)*arg6,*arg7);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
-  bool arg2 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  bool val2 ;
-  int ecode2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("URDFParserOptions_addSensorFramesAsAdditionalFrames_set",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "URDFParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "2"" of type '" "bool""'");
+  arg4 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp4);
+  ecode5 = SWIG_AsVal_int(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "5"" of type '" "iDynTree::FrameIndex""'");
   } 
-  arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->addSensorFramesAsAdditionalFrames = arg2;
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("URDFParserOptions_addSensorFramesAsAdditionalFrames_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_addSensorFramesAsAdditionalFrames_get" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
-  result = (bool) ((arg1)->addSensorFramesAsAdditionalFrames);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_URDFParserOptions_originalFilename_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
-  std::string *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("URDFParserOptions_originalFilename_set",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_originalFilename_set" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "URDFParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "URDFParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
-  }
-  if (arg1) (arg1)->originalFilename = *arg2;
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  return 0;
-fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  return 1;
-}
-
-
-int _wrap_URDFParserOptions_originalFilename_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  std::string *result = 0 ;
-  
-  if (!SWIG_check_num_args("URDFParserOptions_originalFilename_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "URDFParserOptions_originalFilename_get" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+  temp5 = static_cast< iDynTree::FrameIndex >(val5);
+  arg5 = &temp5;
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
-  result = (std::string *) & ((arg1)->originalFilename);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_URDFParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::URDFParserOptions *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_URDFParserOptions",argc,0,0,0)) {
-    SWIG_fail;
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
   }
-  (void)argv;
-  result = (iDynTree::URDFParserOptions *)new iDynTree::URDFParserOptions();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__URDFParserOptions, 1 |  0 );
+  arg6 = reinterpret_cast< iDynTree::Vector3 * >(argp6);
+  result = (bool)(arg1)->updateKinematicsFromFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::JointDOFsDoubleArray const &)*arg4,(iDynTree::FrameIndex const &)*arg5,(iDynTree::Vector3 const &)*arg6);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -67362,26 +73289,67 @@ int _wrap_new_URDFParserOptions(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_delete_URDFParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::URDFParserOptions *arg1 = (iDynTree::URDFParserOptions *) 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  iDynTree::LinkUnknownWrenchContacts *arg2 = 0 ;
+  iDynTree::SensorsMeasurements *arg3 = 0 ;
+  iDynTree::LinkContactWrenches *arg4 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_URDFParserOptions",argc,1,1,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements",argc,5,5,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__URDFParserOptions, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_URDFParserOptions" "', argument " "1"" of type '" "iDynTree::URDFParserOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::URDFParserOptions * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
+  result = (bool)(arg1)->computeExpectedFTSensorsMeasurements((iDynTree::LinkUnknownWrenchContacts const &)*arg2,*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -67389,664 +73357,527 @@ int _wrap_delete_URDFParserOptions(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_modelFromURDF__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::URDFParserOptions arg3 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  iDynTree::LinkUnknownWrenchContacts *arg2 = 0 ;
+  iDynTree::SensorsMeasurements *arg3 = 0 ;
+  iDynTree::LinkContactWrenches *arg4 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   void *argp3 ;
   int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("modelFromURDF",argc,3,3,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques",argc,5,5,0)) {
     SWIG_fail;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    arg1 = ptr;
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  {
-    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__URDFParserOptions,  0 );
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "modelFromURDF" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'"); 
-    }  
-    if (!argp3) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'");
-    } else {
-      arg3 = *(reinterpret_cast< iDynTree::URDFParserOptions * >(argp3));
-    }
+  arg2 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
   }
-  result = (bool)iDynTree::modelFromURDF((std::string const &)*arg1,*arg2,arg3);
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
+  result = (bool)(arg1)->estimateExtWrenchesAndJointTorques((iDynTree::LinkUnknownWrenchContacts const &)*arg2,(iDynTree::SensorsMeasurements const &)*arg3,*arg4,*arg5);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 1;
 }
 
 
-int _wrap_modelFromURDF__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  double arg2 ;
+  double arg3 ;
+  double arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
+  double val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("modelFromURDF",argc,2,2,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill",argc,4,4,0)) {
     SWIG_fail;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDF" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)iDynTree::modelFromURDF((std::string const &)*arg1,*arg2);
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  ecode4 = SWIG_AsVal_double(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "4"" of type '" "double""'");
+  } 
+  arg4 = static_cast< double >(val4);
+  result = (bool)(arg1)->checkThatTheModelIsStill(arg2,arg3,arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
-  return 1;
-}
-
-
-int _wrap_modelFromURDF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_modelFromURDF__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 3) {
-    int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__URDFParserOptions, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_modelFromURDF__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'modelFromURDF'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::modelFromURDF(std::string const &,iDynTree::Model &,iDynTree::URDFParserOptions const)\n"
-    "    iDynTree::modelFromURDF(std::string const &,iDynTree::Model &)\n");
   return 1;
 }
 
 
-int _wrap_modelFromURDFString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::URDFParserOptions arg3 ;
-  int res1 = SWIG_OLDOBJ ;
+int _wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+  iDynTree::LinkNetTotalWrenchesWithoutGravity *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("modelFromURDFString",argc,3,3,0)) {
+  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity",argc,2,2,0)) {
     SWIG_fail;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    arg1 = ptr;
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
   }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  {
-    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__URDFParserOptions,  0 );
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "modelFromURDFString" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'"); 
-    }  
-    if (!argp3) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "3"" of type '" "iDynTree::URDFParserOptions const""'");
-    } else {
-      arg3 = *(reinterpret_cast< iDynTree::URDFParserOptions * >(argp3));
-    }
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
   }
-  result = (bool)iDynTree::modelFromURDFString((std::string const &)*arg1,*arg2,arg3);
+  arg2 = reinterpret_cast< iDynTree::LinkNetTotalWrenchesWithoutGravity * >(argp2);
+  result = (bool)(arg1)->estimateLinkNetWrenchesWithoutGravity(*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 1;
 }
 
 
-int _wrap_modelFromURDFString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+int _wrap_new_SimpleLeggedOdometry(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  bool result;
+  iDynTree::SimpleLeggedOdometry *result = 0 ;
   
-  if (!SWIG_check_num_args("modelFromURDFString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("new_SimpleLeggedOdometry",argc,0,0,0)) {
     SWIG_fail;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "modelFromURDFString" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)iDynTree::modelFromURDFString((std::string const &)*arg1,*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  (void)argv;
+  result = (iDynTree::SimpleLeggedOdometry *)new iDynTree::SimpleLeggedOdometry();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
-  return 1;
-}
-
-
-int _wrap_modelFromURDFString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_modelFromURDFString__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 3) {
-    int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__URDFParserOptions, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_modelFromURDFString__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'modelFromURDFString'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::modelFromURDFString(std::string const &,iDynTree::Model &,iDynTree::URDFParserOptions const)\n"
-    "    iDynTree::modelFromURDFString(std::string const &,iDynTree::Model &)\n");
   return 1;
 }
 
 
-int _wrap_dofsListFromURDF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  std::vector< std::string,std::allocator< std::string > > *arg2 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+int _wrap_delete_SimpleLeggedOdometry(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("dofsListFromURDF",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_SimpleLeggedOdometry",argc,1,1,0)) {
     SWIG_fail;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dofsListFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dofsListFromURDF" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_SimpleLeggedOdometry" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDF" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg2 = reinterpret_cast< std::vector< std::string,std::allocator< std::string > > * >(argp2);
-  result = (bool)iDynTree::dofsListFromURDF((std::string const &)*arg1,*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 1;
 }
 
 
-int _wrap_dofsListFromURDFString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  std::vector< std::string,std::allocator< std::string > > *arg2 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
+int _wrap_SimpleLeggedOdometry_setModel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("dofsListFromURDFString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_setModel",argc,2,2,0)) {
     SWIG_fail;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dofsListFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    arg1 = ptr;
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_setModel" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t,  0 );
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dofsListFromURDFString" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_setModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dofsListFromURDFString" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_setModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< std::string,std::allocator< std::string > > * >(argp2);
-  result = (bool)iDynTree::dofsListFromURDFString((std::string const &)*arg1,*arg2);
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)(arg1)->setModel((iDynTree::Model const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 1;
 }
 
 
-int _wrap_sensorsFromURDF__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::SensorsList *arg2 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+int _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  std::string arg2 ;
+  std::string arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("sensorsFromURDF",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFile",argc,3,3,0)) {
     SWIG_fail;
   }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
     }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "3"" of type '" "std::string const""'"); 
+    }
+    arg3 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
-  result = (bool)iDynTree::sensorsFromURDF((std::string const &)*arg1,*arg2);
+  result = (bool)(arg1)->loadModelFromFile(arg2,arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 1;
 }
 
 
-int _wrap_sensorsFromURDF__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::SensorsList *arg3 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
+int _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  std::string arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("sensorsFromURDF",argc,3,3,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFile",argc,2,2,0)) {
     SWIG_fail;
   }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "1"" of type '" "std::string const &""'"); 
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
     }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "sensorsFromURDF" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDF" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
-  result = (bool)iDynTree::sensorsFromURDF((std::string const &)*arg1,(iDynTree::Model const &)*arg2,*arg3);
+  result = (bool)(arg1)->loadModelFromFile(arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
   return 1;
 }
 
 
-int _wrap_sensorsFromURDF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_SimpleLeggedOdometry_loadModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_sensorsFromURDF__SWIG_0(resc,resv,argc,argv);
+        return _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 3) {
     int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
         _v = SWIG_CheckState(res);
         if (_v) {
-          return _wrap_sensorsFromURDF__SWIG_1(resc,resv,argc,argv);
+          return _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_0(resc,resv,argc,argv);
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'sensorsFromURDF'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_loadModelFromFile'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::sensorsFromURDF(std::string const &,iDynTree::SensorsList &)\n"
-    "    iDynTree::sensorsFromURDF(std::string const &,iDynTree::Model const &,iDynTree::SensorsList &)\n");
+    "    iDynTree::SimpleLeggedOdometry::loadModelFromFile(std::string const,std::string const)\n"
+    "    iDynTree::SimpleLeggedOdometry::loadModelFromFile(std::string const)\n");
   return 1;
 }
 
 
-int _wrap_sensorsFromURDFString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::SensorsList *arg2 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+int _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  std::string arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("sensorsFromURDFString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs",argc,4,4,0)) {
     SWIG_fail;
   }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
     }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+    arg3 = ptr;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::SensorsList &""'"); 
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::SensorsList * >(argp2);
-  result = (bool)iDynTree::sensorsFromURDFString((std::string const &)*arg1,*arg2);
+  result = (bool)(arg1)->loadModelFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_sensorsFromURDFString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  std::string *arg1 = 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::SensorsList *arg3 = 0 ;
-  int res1 = SWIG_OLDOBJ ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
+int _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  std::string arg2 ;
+  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("sensorsFromURDFString",argc,3,3,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs",argc,3,3,0)) {
     SWIG_fail;
   }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
-    res1 = SWIG_AsPtr_std_string(argv[0], &ptr);
-    if (!SWIG_IsOK(res1)) {
-      SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
+    }
+    arg2 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res3 = swig::asptr(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "1"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
     }
-    arg1 = ptr;
-  }
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "sensorsFromURDFString" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "sensorsFromURDFString" "', argument " "3"" of type '" "iDynTree::SensorsList &""'"); 
+    arg3 = ptr;
   }
-  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
-  result = (bool)iDynTree::sensorsFromURDFString((std::string const &)*arg1,(iDynTree::Model const &)*arg2,*arg3);
+  result = (bool)(arg1)->loadModelFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res1)) delete arg1;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res1)) delete arg1;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_sensorsFromURDFString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
     int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_sensorsFromURDFString__SWIG_0(resc,resv,argc,argv);
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_1(resc,resv,argc,argv);
+        }
       }
     }
   }
-  if (argc == 3) {
+  if (argc == 4) {
     int _v;
-    int res = SWIG_AsPtr_std_string(argv[0], (std::string**)(0));
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
         _v = SWIG_CheckState(res);
         if (_v) {
-          return _wrap_sensorsFromURDFString__SWIG_1(resc,resv,argc,argv);
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_0(resc,resv,argc,argv);
+          }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'sensorsFromURDFString'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::sensorsFromURDFString(std::string const &,iDynTree::SensorsList &)\n"
-    "    iDynTree::sensorsFromURDFString(std::string const &,iDynTree::Model const &,iDynTree::SensorsList &)\n");
+    "    iDynTree::SimpleLeggedOdometry::loadModelFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
+    "    iDynTree::SimpleLeggedOdometry::loadModelFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
   return 1;
 }
 
 
-int _wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
-  bool arg2 ;
+int _wrap_SimpleLeggedOdometry_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  bool val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("ModelParserOptions_addSensorFramesAsAdditionalFrames_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_model",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_model" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ModelParserOptions_addSensorFramesAsAdditionalFrames_set" "', argument " "2"" of type '" "bool""'");
-  } 
-  arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->addSensorFramesAsAdditionalFrames = arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  result = (iDynTree::Model *) &((iDynTree::SimpleLeggedOdometry const *)arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -68054,22 +73885,33 @@ int _wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(int resc, mxA
 }
 
 
-int _wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+int _wrap_SimpleLeggedOdometry_updateKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelParserOptions_addSensorFramesAsAdditionalFrames_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_updateKinematics",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_addSensorFramesAsAdditionalFrames_get" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_updateKinematics" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
-  result = (bool) ((arg1)->addSensorFramesAsAdditionalFrames);
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_updateKinematics" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_updateKinematics" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  result = (bool)(arg1)->updateKinematics(*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -68078,35 +73920,50 @@ int _wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(int resc, mxA
 }
 
 
-int _wrap_ModelParserOptions_originalFilename_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+int _wrap_SimpleLeggedOdometry_init__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
   std::string *arg2 = 0 ;
+  iDynTree::Transform arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ModelParserOptions_originalFilename_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_originalFilename_set" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
     res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
     if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelParserOptions_originalFilename_set" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     arg2 = ptr;
   }
-  if (arg1) (arg1)->originalFilename = *arg2;
-  _out = (mxArray*)0;
+  {
+    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'"); 
+    }  
+    if (!argp3) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'");
+    } else {
+      arg3 = *(reinterpret_cast< iDynTree::Transform * >(argp3));
+    }
+  }
+  result = (bool)(arg1)->init((std::string const &)*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
@@ -68116,135 +73973,84 @@ int _wrap_ModelParserOptions_originalFilename_set(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_ModelParserOptions_originalFilename_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  std::string *result = 0 ;
-  
-  if (!SWIG_check_num_args("ModelParserOptions_originalFilename_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelParserOptions_originalFilename_get" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
-  result = (std::string *) & ((arg1)->originalFilename);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_ModelParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::ModelParserOptions *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_ModelParserOptions",argc,0,0,0)) {
-    SWIG_fail;
-  }
-  (void)argv;
-  result = (iDynTree::ModelParserOptions *)new iDynTree::ModelParserOptions();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelParserOptions, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_delete_ModelParserOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelParserOptions *arg1 = (iDynTree::ModelParserOptions *) 0 ;
+int _wrap_SimpleLeggedOdometry_init__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_ModelParserOptions",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelParserOptions, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelParserOptions" "', argument " "1"" of type '" "iDynTree::ModelParserOptions *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_ModelLoader(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::ModelLoader *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_ModelLoader",argc,0,0,0)) {
-    SWIG_fail;
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  (void)argv;
-  result = (iDynTree::ModelLoader *)new iDynTree::ModelLoader();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelLoader, 1 |  0 );
+  result = (bool)(arg1)->init((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_delete_ModelLoader(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_SimpleLeggedOdometry_init__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  iDynTree::Transform arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_ModelLoader",argc,1,1,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelLoader" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  if (is_owned) {
-    delete arg1;
-  }
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_ModelLoader_parsingOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::ModelParserOptions *result = 0 ;
-  
-  if (!SWIG_check_num_args("ModelLoader_parsingOptions",argc,1,1,0)) {
-    SWIG_fail;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_parsingOptions" "', argument " "1"" of type '" "iDynTree::ModelLoader const *""'"); 
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  {
+    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'"); 
+    }  
+    if (!argp3) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'");
+    } else {
+      arg3 = *(reinterpret_cast< iDynTree::Transform * >(argp3));
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  result = (iDynTree::ModelParserOptions *) &((iDynTree::ModelLoader const *)arg1)->parsingOptions();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelParserOptions, 0 |  0 );
+  result = (bool)(arg1)->init(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -68252,33 +74058,31 @@ int _wrap_ModelLoader_parsingOptions(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_ModelLoader_setParsingOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  iDynTree::ModelParserOptions *arg2 = 0 ;
+int _wrap_SimpleLeggedOdometry_init__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_setParsingOptions",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_setParsingOptions" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__ModelParserOptions,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_setParsingOptions" "', argument " "2"" of type '" "iDynTree::ModelParserOptions const &""'"); 
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_setParsingOptions" "', argument " "2"" of type '" "iDynTree::ModelParserOptions const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::ModelParserOptions * >(argp2);
-  (arg1)->setParsingOptions((iDynTree::ModelParserOptions const &)*arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = (bool)(arg1)->init(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -68286,33 +74090,36 @@ int _wrap_ModelLoader_setParsingOptions(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_ModelLoader_loadModelFromString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_SimpleLeggedOdometry_init__SWIG_4(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
   std::string *arg2 = 0 ;
   std::string *arg3 = 0 ;
+  iDynTree::Transform arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int res2 = SWIG_OLDOBJ ;
   int res3 = SWIG_OLDOBJ ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadModelFromString",argc,3,3,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
     res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
     if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     arg2 = ptr;
   }
@@ -68320,14 +74127,25 @@ int _wrap_ModelLoader_loadModelFromString__SWIG_0(int resc, mxArray *resv[], int
     std::string *ptr = (std::string *)0;
     res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
     if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadModelFromString" "', argument " "3"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromString" "', argument " "3"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
     }
     arg3 = ptr;
   }
-  result = (bool)(arg1)->loadModelFromString((std::string const &)*arg2,(std::string const &)*arg3);
+  {
+    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
+    if (!SWIG_IsOK(res4)) {
+      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'"); 
+    }  
+    if (!argp4) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'");
+    } else {
+      arg4 = *(reinterpret_cast< iDynTree::Transform * >(argp4));
+    }
+  }
+  result = (bool)(arg1)->init((std::string const &)*arg2,(std::string const &)*arg3,arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   if (SWIG_IsNewObj(res2)) delete arg2;
@@ -68340,63 +74158,246 @@ int _wrap_ModelLoader_loadModelFromString__SWIG_0(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_ModelLoader_loadModelFromString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_SimpleLeggedOdometry_init__SWIG_5(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
   std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadModelFromString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
     res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
     if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     arg2 = ptr;
   }
-  result = (bool)(arg1)->loadModelFromString((std::string const &)*arg2);
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->init((std::string const &)*arg2,(std::string const &)*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
   if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadModelFromString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_SimpleLeggedOdometry_init__SWIG_6(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  iDynTree::FrameIndex arg3 ;
+  iDynTree::Transform arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::FrameIndex >(val3);
+  {
+    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
+    if (!SWIG_IsOK(res4)) {
+      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'"); 
+    }  
+    if (!argp4) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'");
+    } else {
+      arg4 = *(reinterpret_cast< iDynTree::Transform * >(argp4));
+    }
+  }
+  result = (bool)(arg1)->init(arg2,arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_SimpleLeggedOdometry_init__SWIG_7(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  iDynTree::FrameIndex arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::FrameIndex >(val3);
+  result = (bool)(arg1)->init(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_SimpleLeggedOdometry_init(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        return _wrap_SimpleLeggedOdometry_init__SWIG_3(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_ModelLoader_loadModelFromString__SWIG_1(resc,resv,argc,argv);
+        return _wrap_SimpleLeggedOdometry_init__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
   if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_SimpleLeggedOdometry_init__SWIG_2(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          return _wrap_SimpleLeggedOdometry_init__SWIG_7(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_SimpleLeggedOdometry_init__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
@@ -68405,642 +74406,517 @@ int _wrap_ModelLoader_loadModelFromString(int resc, mxArray *resv[], int argc, m
         int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
         _v = SWIG_CheckState(res);
         if (_v) {
-          return _wrap_ModelLoader_loadModelFromString__SWIG_0(resc,resv,argc,argv);
+          return _wrap_SimpleLeggedOdometry_init__SWIG_5(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_int(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_SimpleLeggedOdometry_init__SWIG_6(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_SimpleLeggedOdometry_init__SWIG_4(resc,resv,argc,argv);
+          }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadModelFromString'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_init'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelLoader::loadModelFromString(std::string const &,std::string const &)\n"
-    "    iDynTree::ModelLoader::loadModelFromString(std::string const &)\n");
+    "    iDynTree::SimpleLeggedOdometry::init(std::string const &,iDynTree::Transform const)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(std::string const &)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const,iDynTree::Transform const)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(std::string const &,std::string const &,iDynTree::Transform const)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(std::string const &,std::string const &)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const,iDynTree::FrameIndex const,iDynTree::Transform const)\n"
+    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const,iDynTree::FrameIndex const)\n");
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
   std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadModelFromFile",argc,3,3,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_changeFixedFrame",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
   {
     std::string *ptr = (std::string *)0;
     res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
     if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "std::string const &""'"); 
     }
     arg2 = ptr;
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadModelFromFile" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromFile" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  result = (bool)(arg1)->loadModelFromFile((std::string const &)*arg2,(std::string const &)*arg3);
+  result = (bool)(arg1)->changeFixedFrame((std::string const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
   if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadModelFromFile",argc,2,2,0)) {
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_changeFixedFrame",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
   }
-  result = (bool)(arg1)->loadModelFromFile((std::string const &)*arg2);
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = (bool)(arg1)->changeFixedFrame(arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_SimpleLeggedOdometry_changeFixedFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
       if (_v) {
-        return _wrap_ModelLoader_loadModelFromFile__SWIG_1(resc,resv,argc,argv);
+        return _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_1(resc,resv,argc,argv);
       }
     }
   }
-  if (argc == 3) {
+  if (argc == 2) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
       _v = SWIG_CheckState(res);
       if (_v) {
-        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_ModelLoader_loadModelFromFile__SWIG_0(resc,resv,argc,argv);
-        }
+        return _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_0(resc,resv,argc,argv);
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadModelFromFile'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_changeFixedFrame'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelLoader::loadModelFromFile(std::string const &,std::string const &)\n"
-    "    iDynTree::ModelLoader::loadModelFromFile(std::string const &)\n");
+    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(std::string const &)\n"
+    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(iDynTree::FrameIndex const)\n");
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
-  std::string arg4 ;
+int _wrap_SimpleLeggedOdometry_getCurrentFixedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  std::string result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_getCurrentFixedLink",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_getCurrentFixedLink" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  result = (arg1)->getCurrentFixedLink();
+  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_SimpleLeggedOdometry_getWorldLinkTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  iDynTree::Transform result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_getWorldLinkTransform",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_getWorldLinkTransform" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_getWorldLinkTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  result = (arg1)->getWorldLinkTransform(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_isLinkBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariablesTypes arg1 ;
+  int val1 ;
+  int ecode1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("isLinkBerdyDynamicVariable",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "isLinkBerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg1 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val1);
+  result = (bool)iDynTree::isLinkBerdyDynamicVariable(arg1);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_isJointBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariablesTypes arg1 ;
+  int val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFullModel",argc,4,4,0)) {
+  if (!SWIG_check_num_args("isJointBerdyDynamicVariable",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "4"" of type '" "std::string const""'"); 
-    }
-    arg4 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  result = (bool)(arg1)->loadReducedModelFromFullModel((iDynTree::Model const &)*arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "isJointBerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg1 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val1);
+  result = (bool)iDynTree::isJointBerdyDynamicVariable(arg1);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
+int _wrap_isDOFBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariablesTypes arg1 ;
+  int val1 ;
+  int ecode1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFullModel",argc,3,3,0)) {
+  if (!SWIG_check_num_args("isDOFBerdyDynamicVariable",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFullModel" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  result = (bool)(arg1)->loadReducedModelFromFullModel((iDynTree::Model const &)*arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
+  ecode1 = SWIG_AsVal_int(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "isDOFBerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg1 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val1);
+  result = (bool)iDynTree::isDOFBerdyDynamicVariable(arg1);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromFullModel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_ModelLoader_loadReducedModelFromFullModel__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
-  }
+int _wrap_new_BerdyOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::BerdyOptions *result = 0 ;
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadReducedModelFromFullModel'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelLoader::loadReducedModelFromFullModel(iDynTree::Model const &,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
-    "    iDynTree::ModelLoader::loadReducedModelFromFullModel(iDynTree::Model const &,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  if (!SWIG_check_num_args("new_BerdyOptions",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::BerdyOptions *)new iDynTree::BerdyOptions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdyOptions, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
-  std::string arg4 ;
+int _wrap_BerdyOptions_berdyVariant_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  iDynTree::BerdyVariants arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromString",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_berdyVariant_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "4"" of type '" "std::string const""'"); 
-    }
-    arg4 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_berdyVariant_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  result = (bool)(arg1)->loadReducedModelFromString(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_berdyVariant_set" "', argument " "2"" of type '" "iDynTree::BerdyVariants""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdyVariants >(val2);
+  if (arg1) (arg1)->berdyVariant = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+int _wrap_BerdyOptions_berdyVariant_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
-  bool result;
+  iDynTree::BerdyVariants result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromString",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_berdyVariant_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromString" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_berdyVariant_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  result = (bool)(arg1)->loadReducedModelFromString(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (iDynTree::BerdyVariants) ((arg1)->berdyVariant);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_ModelLoader_loadReducedModelFromString__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
+int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  bool arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_ModelLoader_loadReducedModelFromString__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadReducedModelFromString'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelLoader::loadReducedModelFromString(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
-    "    iDynTree::ModelLoader::loadReducedModelFromString(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->includeAllNetExternalWrenchesAsDynamicVariables = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
-  std::string arg4 ;
+int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFile",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "4"" of type '" "std::string const""'"); 
-    }
-    arg4 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  result = (bool)(arg1)->loadReducedModelFromFile(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (bool) ((arg1)->includeAllNetExternalWrenchesAsDynamicVariables);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+int _wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_loadReducedModelFromFile",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeAllJointAccelerationsAsSensors_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelLoader_loadReducedModelFromFile" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointAccelerationsAsSensors_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  result = (bool)(arg1)->loadReducedModelFromFile(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllJointAccelerationsAsSensors_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->includeAllJointAccelerationsAsSensors = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ModelLoader_loadReducedModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_ModelLoader_loadReducedModelFromFile__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
+int _wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("BerdyOptions_includeAllJointAccelerationsAsSensors_get",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelLoader, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_ModelLoader_loadReducedModelFromFile__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointAccelerationsAsSensors_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelLoader_loadReducedModelFromFile'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ModelLoader::loadReducedModelFromFile(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
-    "    iDynTree::ModelLoader::loadReducedModelFromFile(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (bool) ((arg1)->includeAllJointAccelerationsAsSensors);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_ModelLoader_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("ModelLoader_model",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeAllJointTorquesAsSensors_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_model" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointTorquesAsSensors_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  result = (iDynTree::Model *) &(arg1)->model();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllJointTorquesAsSensors_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->includeAllJointTorquesAsSensors = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69048,23 +74924,23 @@ int _wrap_ModelLoader_model(int resc, mxArray *resv[], int argc, mxArray *argv[]
 }
 
 
-int _wrap_ModelLoader_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SensorsList *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_sensors",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeAllJointTorquesAsSensors_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_sensors" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointTorquesAsSensors_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  result = (iDynTree::SensorsList *) &(arg1)->sensors();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (bool) ((arg1)->includeAllJointTorquesAsSensors);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69072,23 +74948,30 @@ int _wrap_ModelLoader_sensors(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_ModelLoader_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ModelLoader *arg1 = (iDynTree::ModelLoader *) 0 ;
+int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ModelLoader_isValid",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsSensors_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelLoader, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelLoader_isValid" "', argument " "1"" of type '" "iDynTree::ModelLoader *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ModelLoader * >(argp1);
-  result = (bool)(arg1)->isValid();
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->includeAllNetExternalWrenchesAsSensors = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69096,16 +74979,23 @@ int _wrap_ModelLoader_isValid(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_new_UnknownWrenchContact__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,0,0,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsSensors_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (bool) ((arg1)->includeAllNetExternalWrenchesAsSensors);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69113,66 +75003,30 @@ int _wrap_new_UnknownWrenchContact__SWIG_0(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_UnknownWrenchContact__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContactType arg1 ;
-  iDynTree::Position *arg2 = 0 ;
-  iDynTree::Direction *arg3 = 0 ;
-  iDynTree::Wrench *arg4 = 0 ;
-  unsigned long *arg5 = 0 ;
-  int val1 ;
-  int ecode1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  unsigned long temp5 ;
-  unsigned long val5 ;
-  int ecode5 = 0 ;
+int _wrap_BerdyOptions_includeFixedBaseExternalWrench_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  bool arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
   
-  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,5,5,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeFixedBaseExternalWrench_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
-  } 
-  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Wrench,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeFixedBaseExternalWrench_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::Wrench * >(argp4);
-  ecode5 = SWIG_AsVal_unsigned_SS_long(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "new_UnknownWrenchContact" "', argument " "5"" of type '" "unsigned long""'");
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeFixedBaseExternalWrench_set" "', argument " "2"" of type '" "bool""'");
   } 
-  temp5 = static_cast< unsigned long >(val5);
-  arg5 = &temp5;
-  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3,(iDynTree::Wrench const &)*arg4,(unsigned long const &)*arg5);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->includeFixedBaseExternalWrench = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69180,56 +75034,23 @@ int _wrap_new_UnknownWrenchContact__SWIG_1(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_UnknownWrenchContact__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContactType arg1 ;
-  iDynTree::Position *arg2 = 0 ;
-  iDynTree::Direction *arg3 = 0 ;
-  iDynTree::Wrench *arg4 = 0 ;
-  int val1 ;
-  int ecode1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
+int _wrap_BerdyOptions_includeFixedBaseExternalWrench_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_includeFixedBaseExternalWrench_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
-  } 
-  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Wrench,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "4"" of type '" "iDynTree::Wrench const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeFixedBaseExternalWrench_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::Wrench * >(argp4);
-  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3,(iDynTree::Wrench const &)*arg4);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (bool) ((arg1)->includeFixedBaseExternalWrench);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69237,45 +75058,30 @@ int _wrap_new_UnknownWrenchContact__SWIG_2(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_UnknownWrenchContact__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContactType arg1 ;
-  iDynTree::Position *arg2 = 0 ;
-  iDynTree::Direction *arg3 = 0 ;
-  int val1 ;
-  int ecode1 = 0 ;
-  void *argp2 ;
+int _wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  std::vector< std::string,std::allocator< std::string > > *arg2 = (std::vector< std::string,std::allocator< std::string > > *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
   
-  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
-  } 
-  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
-  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  arg2 = reinterpret_cast< std::vector< std::string,std::allocator< std::string > > * >(argp2);
+  if (arg1) (arg1)->jointOnWhichTheInternalWrenchIsMeasured = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69283,34 +75089,23 @@ int _wrap_new_UnknownWrenchContact__SWIG_3(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_UnknownWrenchContact__SWIG_4(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContactType arg1 ;
-  iDynTree::Position *arg2 = 0 ;
-  int val1 ;
-  int ecode1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+int _wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
+  std::vector< std::string,std::allocator< std::string > > *result = 0 ;
   
-  if (!SWIG_check_num_args("new_UnknownWrenchContact",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContactType""'");
-  } 
-  arg1 = static_cast< iDynTree::UnknownWrenchContactType >(val1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_UnknownWrenchContact" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
-  result = (iDynTree::UnknownWrenchContact *)new iDynTree::UnknownWrenchContact(arg1,(iDynTree::Position const &)*arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (std::vector< std::string,std::allocator< std::string > > *)& ((arg1)->jointOnWhichTheInternalWrenchIsMeasured);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69318,161 +75113,61 @@ int _wrap_new_UnknownWrenchContact__SWIG_4(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_UnknownWrenchContact(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_UnknownWrenchContact__SWIG_0(resc,resv,argc,argv);
-  }
-  if (argc == 2) {
-    int _v;
-    {
-      int res = SWIG_AsVal_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_new_UnknownWrenchContact__SWIG_4(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 3) {
-    int _v;
-    {
-      int res = SWIG_AsVal_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_new_UnknownWrenchContact__SWIG_3(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  if (argc == 4) {
-    int _v;
-    {
-      int res = SWIG_AsVal_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Wrench, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_new_UnknownWrenchContact__SWIG_2(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
-  }
-  if (argc == 5) {
-    int _v;
-    {
-      int res = SWIG_AsVal_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Wrench, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            {
-              int res = SWIG_AsVal_unsigned_SS_long(argv[4], NULL);
-              _v = SWIG_CheckState(res);
-            }
-            if (_v) {
-              return _wrap_new_UnknownWrenchContact__SWIG_1(resc,resv,argc,argv);
-            }
-          }
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_UnknownWrenchContact'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact()\n"
-    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &,iDynTree::Direction const &,iDynTree::Wrench const &,unsigned long const &)\n"
-    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &,iDynTree::Direction const &,iDynTree::Wrench const &)\n"
-    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &,iDynTree::Direction const &)\n"
-    "    iDynTree::UnknownWrenchContact::UnknownWrenchContact(iDynTree::UnknownWrenchContactType const,iDynTree::Position const &)\n");
-  return 1;
-}
-
-
-int _wrap_UnknownWrenchContact_unknownType_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
-  iDynTree::UnknownWrenchContactType arg2 ;
+int _wrap_BerdyOptions_baseLink_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_unknownType_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_baseLink_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_unknownType_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_baseLink_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "UnknownWrenchContact_unknownType_set" "', argument " "2"" of type '" "iDynTree::UnknownWrenchContactType""'");
-  } 
-  arg2 = static_cast< iDynTree::UnknownWrenchContactType >(val2);
-  if (arg1) (arg1)->unknownType = arg2;
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyOptions_baseLink_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyOptions_baseLink_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  if (arg1) (arg1)->baseLink = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_UnknownWrenchContact_unknownType_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+int _wrap_BerdyOptions_baseLink_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContactType result;
+  std::string *result = 0 ;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_unknownType_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_baseLink_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_unknownType_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_baseLink_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  result = (iDynTree::UnknownWrenchContactType) ((arg1)->unknownType);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (std::string *) & ((arg1)->baseLink);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69480,30 +75175,23 @@ int _wrap_UnknownWrenchContact_unknownType_get(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_UnknownWrenchContact_contactPoint_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
-  iDynTree::Position *arg2 = (iDynTree::Position *) 0 ;
+int _wrap_BerdyOptions_checkConsistency(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_contactPoint_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyOptions_checkConsistency",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactPoint_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Position, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "UnknownWrenchContact_contactPoint_set" "', argument " "2"" of type '" "iDynTree::Position *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_checkConsistency" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
-  if (arg1) (arg1)->contactPoint = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  result = (bool)(arg1)->checkConsistency();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69511,23 +75199,26 @@ int _wrap_UnknownWrenchContact_contactPoint_set(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_UnknownWrenchContact_contactPoint_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+int _wrap_delete_BerdyOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Position *result = 0 ;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_contactPoint_get",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_BerdyOptions",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactPoint_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdyOptions" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  result = (iDynTree::Position *)& ((arg1)->contactPoint);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Position, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69535,29 +75226,29 @@ int _wrap_UnknownWrenchContact_contactPoint_get(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_UnknownWrenchContact_forceDirection_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
-  iDynTree::Direction *arg2 = (iDynTree::Direction *) 0 ;
+int _wrap_BerdySensor_type_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+  iDynTree::BerdySensorTypes arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_forceDirection_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySensor_type_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_forceDirection_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Direction, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "UnknownWrenchContact_forceDirection_set" "', argument " "2"" of type '" "iDynTree::Direction *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_type_set" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Direction * >(argp2);
-  if (arg1) (arg1)->forceDirection = *arg2;
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdySensor_type_set" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
+  if (arg1) (arg1)->type = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -69566,23 +75257,23 @@ int _wrap_UnknownWrenchContact_forceDirection_set(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_UnknownWrenchContact_forceDirection_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+int _wrap_BerdySensor_type_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Direction *result = 0 ;
+  iDynTree::BerdySensorTypes result;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_forceDirection_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySensor_type_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_forceDirection_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_type_get" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  result = (iDynTree::Direction *)& ((arg1)->forceDirection);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Direction, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  result = (iDynTree::BerdySensorTypes) ((arg1)->type);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69590,54 +75281,61 @@ int _wrap_UnknownWrenchContact_forceDirection_get(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_UnknownWrenchContact_knownWrench_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
-  iDynTree::Wrench *arg2 = (iDynTree::Wrench *) 0 ;
+int _wrap_BerdySensor_id_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_knownWrench_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySensor_id_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_knownWrench_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_id_set" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "UnknownWrenchContact_knownWrench_set" "', argument " "2"" of type '" "iDynTree::Wrench *""'"); 
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySensor_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Wrench * >(argp2);
-  if (arg1) (arg1)->knownWrench = *arg2;
+  if (arg1) (arg1)->id = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_UnknownWrenchContact_knownWrench_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+int _wrap_BerdySensor_id_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Wrench *result = 0 ;
+  std::string *result = 0 ;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_knownWrench_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySensor_id_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_knownWrench_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_id_get" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  result = (iDynTree::Wrench *)& ((arg1)->knownWrench);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Wrench, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  result = (std::string *) & ((arg1)->id);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69645,29 +75343,29 @@ int _wrap_UnknownWrenchContact_knownWrench_get(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_UnknownWrenchContact_contactId_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
-  unsigned long arg2 ;
+int _wrap_BerdySensor_range_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+  iDynTree::IndexRange *arg2 = (iDynTree::IndexRange *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned long val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_contactId_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySensor_range_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactId_set" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_range_set" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_long(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "UnknownWrenchContact_contactId_set" "', argument " "2"" of type '" "unsigned long""'");
-  } 
-  arg2 = static_cast< unsigned long >(val2);
-  if (arg1) (arg1)->contactId = arg2;
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_range_set" "', argument " "2"" of type '" "iDynTree::IndexRange *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::IndexRange * >(argp2);
+  if (arg1) (arg1)->range = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -69676,23 +75374,23 @@ int _wrap_UnknownWrenchContact_contactId_set(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_UnknownWrenchContact_contactId_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+int _wrap_BerdySensor_range_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  unsigned long result;
+  iDynTree::IndexRange *result = 0 ;
   
-  if (!SWIG_check_num_args("UnknownWrenchContact_contactId_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySensor_range_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "UnknownWrenchContact_contactId_get" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_range_get" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  result = (unsigned long) ((arg1)->contactId);
-  _out = SWIG_From_unsigned_SS_long(static_cast< unsigned long >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  result = (iDynTree::IndexRange *)& ((arg1)->range);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69700,26 +75398,34 @@ int _wrap_UnknownWrenchContact_contactId_get(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_delete_UnknownWrenchContact(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::UnknownWrenchContact *arg1 = (iDynTree::UnknownWrenchContact *) 0 ;
+int _wrap_BerdySensor_eq(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+  iDynTree::BerdySensor *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_UnknownWrenchContact",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySensor_eq",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__UnknownWrenchContact, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_UnknownWrenchContact" "', argument " "1"" of type '" "iDynTree::UnknownWrenchContact *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_eq" "', argument " "1"" of type '" "iDynTree::BerdySensor const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdySensor,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_eq" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySensor_eq" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::BerdySensor * >(argp2);
+  result = (bool)((iDynTree::BerdySensor const *)arg1)->operator ==((iDynTree::BerdySensor const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69727,23 +75433,34 @@ int _wrap_delete_UnknownWrenchContact(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_new_LinkUnknownWrenchContacts__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  unsigned int arg1 ;
-  unsigned int val1 ;
-  int ecode1 = 0 ;
+int _wrap_BerdySensor_lt(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+  iDynTree::BerdySensor *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::LinkUnknownWrenchContacts *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_LinkUnknownWrenchContacts",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySensor_lt",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_unsigned_SS_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "unsigned int""'");
-  } 
-  arg1 = static_cast< unsigned int >(val1);
-  result = (iDynTree::LinkUnknownWrenchContacts *)new iDynTree::LinkUnknownWrenchContacts(arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_lt" "', argument " "1"" of type '" "iDynTree::BerdySensor const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdySensor,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_lt" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySensor_lt" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::BerdySensor * >(argp2);
+  result = (bool)((iDynTree::BerdySensor const *)arg1)->operator <((iDynTree::BerdySensor const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69751,16 +75468,16 @@ int _wrap_new_LinkUnknownWrenchContacts__SWIG_0(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_LinkUnknownWrenchContacts__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_new_BerdySensor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::LinkUnknownWrenchContacts *result = 0 ;
+  iDynTree::BerdySensor *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkUnknownWrenchContacts",argc,0,0,0)) {
+  if (!SWIG_check_num_args("new_BerdySensor",argc,0,0,0)) {
     SWIG_fail;
   }
   (void)argv;
-  result = (iDynTree::LinkUnknownWrenchContacts *)new iDynTree::LinkUnknownWrenchContacts();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 1 |  0 );
+  result = (iDynTree::BerdySensor *)new iDynTree::BerdySensor();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdySensor, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69768,26 +75485,26 @@ int _wrap_new_LinkUnknownWrenchContacts__SWIG_1(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_LinkUnknownWrenchContacts__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_delete_BerdySensor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::LinkUnknownWrenchContacts *result = 0 ;
   
-  if (!SWIG_check_num_args("new_LinkUnknownWrenchContacts",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_BerdySensor",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdySensor" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
   }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  result = (iDynTree::LinkUnknownWrenchContacts *)new iDynTree::LinkUnknownWrenchContacts((iDynTree::Model const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 1 |  0 );
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69795,54 +75512,29 @@ int _wrap_new_LinkUnknownWrenchContacts__SWIG_2(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_new_LinkUnknownWrenchContacts(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_LinkUnknownWrenchContacts__SWIG_1(resc,resv,argc,argv);
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_new_LinkUnknownWrenchContacts__SWIG_2(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    {
-      int res = SWIG_AsVal_unsigned_SS_int(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
-    if (_v) {
-      return _wrap_new_LinkUnknownWrenchContacts__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_LinkUnknownWrenchContacts'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkUnknownWrenchContacts::LinkUnknownWrenchContacts(unsigned int)\n"
-    "    iDynTree::LinkUnknownWrenchContacts::LinkUnknownWrenchContacts()\n"
-    "    iDynTree::LinkUnknownWrenchContacts::LinkUnknownWrenchContacts(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_LinkUnknownWrenchContacts_clear(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+int _wrap_BerdyDynamicVariable_type_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+  iDynTree::BerdyDynamicVariablesTypes arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_clear",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_type_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_clear" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_type_set" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  (arg1)->clear();
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyDynamicVariable_type_set" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
+  if (arg1) (arg1)->type = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -69851,30 +75543,23 @@ int _wrap_LinkUnknownWrenchContacts_clear(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  unsigned int arg2 ;
+int _wrap_BerdyDynamicVariable_type_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  unsigned int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
+  iDynTree::BerdyDynamicVariablesTypes result;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_type_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_type_get" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  ecode2 = SWIG_AsVal_unsigned_SS_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "2"" of type '" "unsigned int""'");
-  } 
-  arg2 = static_cast< unsigned int >(val2);
-  (arg1)->resize(arg2);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  result = (iDynTree::BerdyDynamicVariablesTypes) ((arg1)->type);
+  _out = SWIG_From_int(static_cast< int >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69882,104 +75567,61 @@ int _wrap_LinkUnknownWrenchContacts_resize__SWIG_0(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_BerdyDynamicVariable_id_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+  std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_id_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_id_set" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_resize" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyDynamicVariable_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  (arg1)->resize((iDynTree::Model const &)*arg2);
+  if (arg1) (arg1)->id = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_LinkUnknownWrenchContacts_resize__SWIG_1(resc,resv,argc,argv);
-      }
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_unsigned_SS_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_LinkUnknownWrenchContacts_resize__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkUnknownWrenchContacts_resize'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkUnknownWrenchContacts::resize(unsigned int)\n"
-    "    iDynTree::LinkUnknownWrenchContacts::resize(iDynTree::Model const &)\n");
-  return 1;
-}
-
-
-int _wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_BerdyDynamicVariable_id_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  size_t result;
+  std::string *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_getNrOfContactsForLink",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_id_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_getNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_id_get" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_getNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = ((iDynTree::LinkUnknownWrenchContacts const *)arg1)->getNrOfContactsForLink(arg2);
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  result = (std::string *) & ((arg1)->id);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -69987,37 +75629,29 @@ int _wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(int resc, mxArray *re
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  size_t arg3 ;
+int _wrap_BerdyDynamicVariable_range_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+  iDynTree::IndexRange *arg2 = (iDynTree::IndexRange *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_setNrOfContactsForLink",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_range_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_setNrOfContactsForLink" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_range_set" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_setNrOfContactsForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_setNrOfContactsForLink" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  (arg1)->setNrOfContactsForLink(arg2,arg3);
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_range_set" "', argument " "2"" of type '" "iDynTree::IndexRange *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::IndexRange * >(argp2);
+  if (arg1) (arg1)->range = *arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -70026,41 +75660,23 @@ int _wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(int resc, mxArray *re
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_addNewContactForLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::UnknownWrenchContact *arg3 = 0 ;
+int _wrap_BerdyDynamicVariable_range_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
   mxArray * _out;
+  iDynTree::IndexRange *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_addNewContactForLink",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_range_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__UnknownWrenchContact,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "3"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewContactForLink" "', argument " "3"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_range_get" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp3);
-  (arg1)->addNewContactForLink(arg2,(iDynTree::UnknownWrenchContact const &)*arg3);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  result = (iDynTree::IndexRange *)& ((arg1)->range);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70068,52 +75684,33 @@ int _wrap_LinkUnknownWrenchContacts_addNewContactForLink(int resc, mxArray *resv
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_addNewContactInFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::FrameIndex arg3 ;
-  iDynTree::UnknownWrenchContact *arg4 = 0 ;
+int _wrap_BerdyDynamicVariable_eq(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+  iDynTree::BerdyDynamicVariable *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_addNewContactInFrame",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_eq",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_eq" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdyDynamicVariable,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_eq" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::FrameIndex >(val3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__UnknownWrenchContact,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "4"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewContactInFrame" "', argument " "4"" of type '" "iDynTree::UnknownWrenchContact const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyDynamicVariable_eq" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::UnknownWrenchContact * >(argp4);
-  result = (bool)(arg1)->addNewContactInFrame((iDynTree::Model const &)*arg2,arg3,(iDynTree::UnknownWrenchContact const &)*arg4);
+  arg2 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp2);
+  result = (bool)((iDynTree::BerdyDynamicVariable const *)arg1)->operator ==((iDynTree::BerdyDynamicVariable const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -70122,41 +75719,33 @@ int _wrap_LinkUnknownWrenchContacts_addNewContactInFrame(int resc, mxArray *resv
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::FrameIndex arg3 ;
+int _wrap_BerdyDynamicVariable_lt(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+  iDynTree::BerdyDynamicVariable *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyDynamicVariable_lt",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_lt" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdyDynamicVariable,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_lt" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyDynamicVariable_lt" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::FrameIndex >(val3);
-  result = (bool)(arg1)->addNewUnknownFullWrenchInFrameOrigin((iDynTree::Model const &)*arg2,arg3);
+  arg2 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp2);
+  result = (bool)((iDynTree::BerdyDynamicVariable const *)arg1)->operator <((iDynTree::BerdyDynamicVariable const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -70165,39 +75754,16 @@ int _wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(int res
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  size_t arg3 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
+int _wrap_new_BerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
+  iDynTree::BerdyDynamicVariable *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_contactWrench",argc,3,3,0)) {
+  if (!SWIG_check_num_args("new_BerdyDynamicVariable",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  result = (iDynTree::UnknownWrenchContact *) &(arg1)->contactWrench(arg2,arg3);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  (void)argv;
+  result = (iDynTree::BerdyDynamicVariable *)new iDynTree::BerdyDynamicVariable();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70205,39 +75771,26 @@ int _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_0(int resc, mxArray *res
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  size_t arg3 ;
+int _wrap_delete_BerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::UnknownWrenchContact *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_contactWrench",argc,3,3,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_BerdyDynamicVariable",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "LinkUnknownWrenchContacts_contactWrench" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  result = (iDynTree::UnknownWrenchContact *) &((iDynTree::LinkUnknownWrenchContacts const *)arg1)->contactWrench(arg2,arg3);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__UnknownWrenchContact, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70245,86 +75798,40 @@ int _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_1(int resc, mxArray *res
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_contactWrench(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_size_t(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_size_t(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_LinkUnknownWrenchContacts_contactWrench__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
+int _wrap_new_BerdyHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::BerdyHelper *result = 0 ;
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'LinkUnknownWrenchContacts_contactWrench'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::LinkUnknownWrenchContacts::contactWrench(iDynTree::LinkIndex const,size_t const)\n"
-    "    iDynTree::LinkUnknownWrenchContacts::contactWrench(iDynTree::LinkIndex const,size_t const) const\n");
+  if (!SWIG_check_num_args("new_BerdyHelper",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::BerdyHelper *)new iDynTree::BerdyHelper();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdyHelper, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_LinkUnknownWrenchContacts_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_BerdyHelper_model__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  std::string result;
+  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("LinkUnknownWrenchContacts_toString",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_model",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "LinkUnknownWrenchContacts_toString" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "LinkUnknownWrenchContacts_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "LinkUnknownWrenchContacts_toString" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_model" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = ((iDynTree::LinkUnknownWrenchContacts const *)arg1)->toString((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (iDynTree::Model *) &(arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70332,26 +75839,23 @@ int _wrap_LinkUnknownWrenchContacts_toString(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_delete_LinkUnknownWrenchContacts(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::LinkUnknownWrenchContacts *arg1 = (iDynTree::LinkUnknownWrenchContacts *) 0 ;
+int _wrap_BerdyHelper_sensors__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::SensorsList *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_LinkUnknownWrenchContacts",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_sensors",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_LinkUnknownWrenchContacts" "', argument " "1"" of type '" "iDynTree::LinkUnknownWrenchContacts *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_sensors" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (iDynTree::SensorsList *) &(arg1)->sensors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70359,16 +75863,23 @@ int _wrap_delete_LinkUnknownWrenchContacts(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_new_estimateExternalWrenchesBuffers__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_BerdyHelper_dynamicTraversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::estimateExternalWrenchesBuffers *result = 0 ;
+  iDynTree::Traversal *result = 0 ;
   
-  if (!SWIG_check_num_args("new_estimateExternalWrenchesBuffers",argc,0,0,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_dynamicTraversal",argc,1,1,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::estimateExternalWrenchesBuffers *)new iDynTree::estimateExternalWrenchesBuffers();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_dynamicTraversal" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (iDynTree::Traversal *) &((iDynTree::BerdyHelper const *)arg1)->dynamicTraversal();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70376,26 +75887,23 @@ int _wrap_new_estimateExternalWrenchesBuffers__SWIG_0(int resc, mxArray *resv[],
 }
 
 
-int _wrap_new_estimateExternalWrenchesBuffers__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SubModelDecomposition *arg1 = 0 ;
-  void *argp1 ;
+int _wrap_BerdyHelper_model__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::estimateExternalWrenchesBuffers *result = 0 ;
+  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("new_estimateExternalWrenchesBuffers",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_model",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_model" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp1);
-  result = (iDynTree::estimateExternalWrenchesBuffers *)new iDynTree::estimateExternalWrenchesBuffers((iDynTree::SubModelDecomposition const &)*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 1 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (iDynTree::Model *) &((iDynTree::BerdyHelper const *)arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70403,31 +75911,51 @@ int _wrap_new_estimateExternalWrenchesBuffers__SWIG_1(int resc, mxArray *resv[],
 }
 
 
-int _wrap_new_estimateExternalWrenchesBuffers__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  size_t arg1 ;
-  size_t arg2 ;
-  size_t val1 ;
-  int ecode1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
+int _wrap_BerdyHelper_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdyHelper_model__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdyHelper_model__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_model'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::BerdyHelper::model()\n"
+    "    iDynTree::BerdyHelper::model() const\n");
+  return 1;
+}
+
+
+int _wrap_BerdyHelper_sensors__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  iDynTree::estimateExternalWrenchesBuffers *result = 0 ;
+  iDynTree::SensorsList *result = 0 ;
   
-  if (!SWIG_check_num_args("new_estimateExternalWrenchesBuffers",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_sensors",argc,1,1,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_size_t(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "size_t""'");
-  } 
-  arg1 = static_cast< size_t >(val1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "new_estimateExternalWrenchesBuffers" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  result = (iDynTree::estimateExternalWrenchesBuffers *)new iDynTree::estimateExternalWrenchesBuffers(arg1,arg2);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_sensors" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (iDynTree::SensorsList *) &((iDynTree::BerdyHelper const *)arg1)->sensors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70435,72 +75963,111 @@ int _wrap_new_estimateExternalWrenchesBuffers__SWIG_2(int resc, mxArray *resv[],
 }
 
 
-int _wrap_new_estimateExternalWrenchesBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 0) {
-    return _wrap_new_estimateExternalWrenchesBuffers__SWIG_0(resc,resv,argc,argv);
-  }
+int _wrap_BerdyHelper_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SubModelDecomposition, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_new_estimateExternalWrenchesBuffers__SWIG_1(resc,resv,argc,argv);
+      return _wrap_BerdyHelper_sensors__SWIG_0(resc,resv,argc,argv);
     }
   }
-  if (argc == 2) {
+  if (argc == 1) {
     int _v;
-    {
-      int res = SWIG_AsVal_size_t(argv[0], NULL);
-      _v = SWIG_CheckState(res);
-    }
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_new_estimateExternalWrenchesBuffers__SWIG_2(resc,resv,argc,argv);
-      }
+      return _wrap_BerdyHelper_sensors__SWIG_1(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'new_estimateExternalWrenchesBuffers'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_sensors'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::estimateExternalWrenchesBuffers::estimateExternalWrenchesBuffers()\n"
-    "    iDynTree::estimateExternalWrenchesBuffers::estimateExternalWrenchesBuffers(iDynTree::SubModelDecomposition const &)\n"
-    "    iDynTree::estimateExternalWrenchesBuffers::estimateExternalWrenchesBuffers(size_t const,size_t const)\n");
+    "    iDynTree::BerdyHelper::sensors()\n"
+    "    iDynTree::BerdyHelper::sensors() const\n");
   return 1;
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_resize__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  iDynTree::SubModelDecomposition *arg2 = 0 ;
+int _wrap_BerdyHelper_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("BerdyHelper_isValid",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_isValid" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (bool)((iDynTree::BerdyHelper const *)arg1)->isValid();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_BerdyHelper_init__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
+  iDynTree::BerdyOptions arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_resize",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_init",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_init" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp2);
-  (arg1)->resize((iDynTree::SubModelDecomposition const &)*arg2);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  {
+    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__BerdyOptions,  0 );
+    if (!SWIG_IsOK(res4)) {
+      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_init" "', argument " "4"" of type '" "iDynTree::BerdyOptions const""'"); 
+    }  
+    if (!argp4) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "4"" of type '" "iDynTree::BerdyOptions const""'");
+    } else {
+      arg4 = *(reinterpret_cast< iDynTree::BerdyOptions * >(argp4));
+    }
+  }
+  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70508,38 +76075,45 @@ int _wrap_estimateExternalWrenchesBuffers_resize__SWIG_0(int resc, mxArray *resv
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_resize__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  size_t arg2 ;
-  size_t arg3 ;
+int _wrap_BerdyHelper_init__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  size_t val2 ;
-  int ecode2 = 0 ;
-  size_t val3 ;
-  int ecode3 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_resize",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_init",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_init" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "2"" of type '" "size_t""'");
-  } 
-  arg2 = static_cast< size_t >(val2);
-  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "estimateExternalWrenchesBuffers_resize" "', argument " "3"" of type '" "size_t""'");
-  } 
-  arg3 = static_cast< size_t >(val3);
-  (arg1)->resize(arg2,arg3);
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70547,68 +76121,76 @@ int _wrap_estimateExternalWrenchesBuffers_resize__SWIG_1(int resc, mxArray *resv
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_resize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_BerdyHelper_init(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
       void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SubModelDecomposition, 0);
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        return _wrap_estimateExternalWrenchesBuffers_resize__SWIG_0(resc,resv,argc,argv);
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_BerdyHelper_init__SWIG_1(resc,resv,argc,argv);
+        }
       }
     }
   }
-  if (argc == 3) {
+  if (argc == 4) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      {
-        int res = SWIG_AsVal_size_t(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_size_t(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        _v = SWIG_CheckState(res);
         if (_v) {
-          return _wrap_estimateExternalWrenchesBuffers_resize__SWIG_1(resc,resv,argc,argv);
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__BerdyOptions, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_BerdyHelper_init__SWIG_0(resc,resv,argc,argv);
+          }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'estimateExternalWrenchesBuffers_resize'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_init'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::estimateExternalWrenchesBuffers::resize(iDynTree::SubModelDecomposition const &)\n"
-    "    iDynTree::estimateExternalWrenchesBuffers::resize(size_t const,size_t const)\n");
+    "    iDynTree::BerdyHelper::init(iDynTree::Model const &,iDynTree::SensorsList const &,iDynTree::BerdyOptions const)\n"
+    "    iDynTree::BerdyHelper::init(iDynTree::Model const &,iDynTree::SensorsList const &)\n");
   return 1;
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::BerdyOptions result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_getNrOfSubModels",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getOptions",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_getNrOfSubModels" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getOptions" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = ((iDynTree::estimateExternalWrenchesBuffers const *)arg1)->getNrOfSubModels();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getOptions();
+  _out = SWIG_NewPointerObj((new iDynTree::BerdyOptions(static_cast< const iDynTree::BerdyOptions& >(result))), SWIGTYPE_p_iDynTree__BerdyOptions, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70616,22 +76198,22 @@ int _wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(int resc, mxArray *re
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_getNrOfLinks(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getNrOfDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   size_t result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_getNrOfLinks",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getNrOfDynamicVariables",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_getNrOfLinks" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNrOfDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = ((iDynTree::estimateExternalWrenchesBuffers const *)arg1)->getNrOfLinks();
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getNrOfDynamicVariables();
   _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -70640,34 +76222,23 @@ int _wrap_estimateExternalWrenchesBuffers_getNrOfLinks(int resc, mxArray *resv[]
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_isConsistent(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  iDynTree::SubModelDecomposition *arg2 = 0 ;
+int _wrap_BerdyHelper_getNrOfDynamicEquations(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_isConsistent",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getNrOfDynamicEquations",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_isConsistent" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesBuffers_isConsistent" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNrOfDynamicEquations" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp2);
-  result = (bool)((iDynTree::estimateExternalWrenchesBuffers const *)arg1)->isConsistent((iDynTree::SubModelDecomposition const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getNrOfDynamicEquations();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70675,30 +76246,23 @@ int _wrap_estimateExternalWrenchesBuffers_isConsistent(int resc, mxArray *resv[]
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_A_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *arg2 = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *) 0 ;
+int _wrap_BerdyHelper_getNrOfSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  size_t result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_A_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getNrOfSensorsMeasurements",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_A_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_A_set" "', argument " "2"" of type '" "std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNrOfSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > * >(argp2);
-  if (arg1) (arg1)->A = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getNrOfSensorsMeasurements();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70706,23 +76270,67 @@ int _wrap_estimateExternalWrenchesBuffers_A_set(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_A_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg4 = 0 ;
+  iDynTree::VectorDynSize *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
-  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_A_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_resizeAndZeroBerdyMatrices",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_A_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *)& ((arg1)->A);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
+  result = (bool)(arg1)->resizeAndZeroBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70730,30 +76338,67 @@ int _wrap_estimateExternalWrenchesBuffers_A_get(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_x_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *arg2 = (std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *) 0 ;
+int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::MatrixDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::MatrixDynSize *arg4 = 0 ;
+  iDynTree::VectorDynSize *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_x_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_resizeAndZeroBerdyMatrices",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_x_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_x_set" "', argument " "2"" of type '" "std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > * >(argp2);
-  if (arg1) (arg1)->x = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
+  result = (bool)(arg1)->resizeAndZeroBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70761,54 +76406,135 @@ int _wrap_estimateExternalWrenchesBuffers_x_set(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_x_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *result = 0 ;
-  
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_x_get",argc,1,1,0)) {
-    SWIG_fail;
+int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_x_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_1(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = (std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *)& ((arg1)->x);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_resizeAndZeroBerdyMatrices'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::BerdyHelper::resizeAndZeroBerdyMatrices(iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &,iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &)\n"
+    "    iDynTree::BerdyHelper::resizeAndZeroBerdyMatrices(iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &,iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &)\n");
   return 1;
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_b_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *arg2 = (std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *) 0 ;
+int _wrap_BerdyHelper_getBerdyMatrices__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg4 = 0 ;
+  iDynTree::VectorDynSize *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getBerdyMatrices",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_b_set" "', argument " "2"" of type '" "std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > * >(argp2);
-  if (arg1) (arg1)->b = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
+  result = (bool)(arg1)->getBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70816,23 +76542,67 @@ int _wrap_estimateExternalWrenchesBuffers_b_set(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_b_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getBerdyMatrices__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::MatrixDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::MatrixDynSize *arg4 = 0 ;
+  iDynTree::VectorDynSize *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
-  std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getBerdyMatrices",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = (std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *)& ((arg1)->b);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
+  result = (bool)(arg1)->getBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70840,30 +76610,91 @@ int _wrap_estimateExternalWrenchesBuffers_b_get(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_pinvA_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *arg2 = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *) 0 ;
+int _wrap_BerdyHelper_getBerdyMatrices(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_BerdyHelper_getBerdyMatrices__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_BerdyHelper_getBerdyMatrices__SWIG_1(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_getBerdyMatrices'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::BerdyHelper::getBerdyMatrices(iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &,iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &)\n"
+    "    iDynTree::BerdyHelper::getBerdyMatrices(iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &,iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &)\n");
+  return 1;
+}
+
+
+int _wrap_BerdyHelper_getSensorsOrdering(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > *result = 0 ;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_pinvA_set",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_pinvA_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+  if (!SWIG_check_num_args("BerdyHelper_getSensorsOrdering",argc,1,1,0)) {
+    SWIG_fail;
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_pinvA_set" "', argument " "2"" of type '" "std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getSensorsOrdering" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > * >(argp2);
-  if (arg1) (arg1)->pinvA = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > *) &((iDynTree::BerdyHelper const *)arg1)->getSensorsOrdering();
+  _out = swig::from(static_cast< std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70871,23 +76702,39 @@ int _wrap_estimateExternalWrenchesBuffers_pinvA_set(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_pinvA_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getRangeSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::SensorType arg2 ;
+  unsigned int arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  unsigned int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *result = 0 ;
+  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_pinvA_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeSensorVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_pinvA_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = (std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *)& ((arg1)->pinvA);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeSensorVariable" "', argument " "2"" of type '" "iDynTree::SensorType""'");
+  } 
+  arg2 = static_cast< iDynTree::SensorType >(val2);
+  ecode3 = SWIG_AsVal_unsigned_SS_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeSensorVariable" "', argument " "3"" of type '" "unsigned int""'");
+  } 
+  arg3 = static_cast< unsigned int >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeSensorVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70895,30 +76742,39 @@ int _wrap_estimateExternalWrenchesBuffers_pinvA_get(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  iDynTree::LinkWrenches *arg2 = (iDynTree::LinkWrenches *) 0 ;
+int _wrap_BerdyHelper_getRangeDOFSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::BerdySensorTypes arg2 ;
+  iDynTree::DOFIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_contacts_subtree_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeDOFSensorVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_contacts_subtree_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_b_contacts_subtree_set" "', argument " "2"" of type '" "iDynTree::LinkWrenches *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeDOFSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkWrenches * >(argp2);
-  if (arg1) (arg1)->b_contacts_subtree = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeDOFSensorVariable" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeDOFSensorVariable" "', argument " "3"" of type '" "iDynTree::DOFIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::DOFIndex >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeDOFSensorVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70926,23 +76782,39 @@ int _wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(int resc, mxArr
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getRangeJointSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::BerdySensorTypes arg2 ;
+  iDynTree::JointIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::LinkWrenches *result = 0 ;
+  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_b_contacts_subtree_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeJointSensorVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_b_contacts_subtree_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeJointSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = (iDynTree::LinkWrenches *)& ((arg1)->b_contacts_subtree);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkWrenches, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeJointSensorVariable" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeJointSensorVariable" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::JointIndex >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeJointSensorVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70950,30 +76822,39 @@ int _wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(int resc, mxArr
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
-  iDynTree::LinkPositions *arg2 = (iDynTree::LinkPositions *) 0 ;
+int _wrap_BerdyHelper_getRangeLinkSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::BerdySensorTypes arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_subModelBase_H_link_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeLinkSensorVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_subModelBase_H_link_set" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesBuffers_subModelBase_H_link_set" "', argument " "2"" of type '" "iDynTree::LinkPositions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeLinkSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkPositions * >(argp2);
-  if (arg1) (arg1)->subModelBase_H_link = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeLinkSensorVariable" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeLinkSensorVariable" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeLinkSensorVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -70981,23 +76862,39 @@ int _wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(int resc, mxAr
 }
 
 
-int _wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getRangeLinkVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::BerdyDynamicVariablesTypes arg2 ;
+  iDynTree::LinkIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::LinkPositions *result = 0 ;
+  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesBuffers_subModelBase_H_link_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeLinkVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesBuffers_subModelBase_H_link_get" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeLinkVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  result = (iDynTree::LinkPositions *)& ((arg1)->subModelBase_H_link);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__LinkPositions, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeLinkVariable" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeLinkVariable" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::LinkIndex >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeLinkVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -71005,26 +76902,39 @@ int _wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(int resc, mxAr
 }
 
 
-int _wrap_delete_estimateExternalWrenchesBuffers(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::estimateExternalWrenchesBuffers *arg1 = (iDynTree::estimateExternalWrenchesBuffers *) 0 ;
+int _wrap_BerdyHelper_getRangeJointVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::BerdyDynamicVariablesTypes arg2 ;
+  iDynTree::JointIndex arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
+  iDynTree::IndexRange result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_estimateExternalWrenchesBuffers",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeJointVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_estimateExternalWrenchesBuffers" "', argument " "1"" of type '" "iDynTree::estimateExternalWrenchesBuffers *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeJointVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeJointVariable" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeJointVariable" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::JointIndex >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeJointVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -71032,103 +76942,39 @@ int _wrap_delete_estimateExternalWrenchesBuffers(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_estimateExternalWrenchesWithoutInternalFT(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::LinkUnknownWrenchContacts *arg3 = 0 ;
-  iDynTree::JointPosDoubleArray *arg4 = 0 ;
-  iDynTree::LinkVelArray *arg5 = 0 ;
-  iDynTree::LinkAccArray *arg6 = 0 ;
-  iDynTree::estimateExternalWrenchesBuffers *arg7 = 0 ;
-  iDynTree::LinkContactWrenches *arg8 = 0 ;
-  void *argp1 ;
+int _wrap_BerdyHelper_getRangeDOFVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::BerdyDynamicVariablesTypes arg2 ;
+  iDynTree::DOFIndex arg3 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
-  void *argp8 = 0 ;
-  int res8 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  int val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("estimateExternalWrenchesWithoutInternalFT",argc,8,8,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getRangeDOFVariable",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "3"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "3"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "5"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkVelArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "6"" of type '" "iDynTree::LinkAccArray const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "6"" of type '" "iDynTree::LinkAccArray const &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkAccArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "7"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "7"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
-  if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "8"" of type '" "iDynTree::LinkContactWrenches &""'"); 
-  }
-  if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenchesWithoutInternalFT" "', argument " "8"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeDOFVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg8 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp8);
-  result = (bool)iDynTree::estimateExternalWrenchesWithoutInternalFT((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::LinkUnknownWrenchContacts const &)*arg3,(iDynTree::JointPosDoubleArray const &)*arg4,(iDynTree::LinkVelArray const &)*arg5,(iDynTree::LinkAccArray const &)*arg6,*arg7,*arg8);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeDOFVariable" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
+  } 
+  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
+  ecode3 = SWIG_AsVal_int(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeDOFVariable" "', argument " "3"" of type '" "iDynTree::DOFIndex""'");
+  } 
+  arg3 = static_cast< iDynTree::DOFIndex >(val3);
+  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeDOFVariable(arg2,arg3);
+  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -71136,125 +76982,23 @@ int _wrap_estimateExternalWrenchesWithoutInternalFT(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_estimateExternalWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::SubModelDecomposition *arg2 = 0 ;
-  iDynTree::SensorsList *arg3 = 0 ;
-  iDynTree::LinkUnknownWrenchContacts *arg4 = 0 ;
-  iDynTree::JointPosDoubleArray *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  iDynTree::LinkAccArray *arg7 = 0 ;
-  iDynTree::SensorsMeasurements *arg8 = 0 ;
-  iDynTree::estimateExternalWrenchesBuffers *arg9 = 0 ;
-  iDynTree::LinkContactWrenches *arg10 = 0 ;
-  void *argp1 ;
+int _wrap_BerdyHelper_getDynamicVariablesOrdering(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
-  void *argp7 ;
-  int res7 = 0 ;
-  void *argp8 ;
-  int res8 = 0 ;
-  void *argp9 = 0 ;
-  int res9 = 0 ;
-  void *argp10 = 0 ;
-  int res10 = 0 ;
   mxArray * _out;
-  bool result;
+  std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > *result = 0 ;
   
-  if (!SWIG_check_num_args("estimateExternalWrenches",argc,10,10,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_getDynamicVariablesOrdering",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SubModelDecomposition,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateExternalWrenches" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "2"" of type '" "iDynTree::SubModelDecomposition const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::SubModelDecomposition * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateExternalWrenches" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "estimateExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "estimateExternalWrenches" "', argument " "5"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "5"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "estimateExternalWrenches" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "6"" of type '" "iDynTree::LinkVelArray const &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "estimateExternalWrenches" "', argument " "7"" of type '" "iDynTree::LinkAccArray const &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "7"" of type '" "iDynTree::LinkAccArray const &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::LinkAccArray * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
-  if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "estimateExternalWrenches" "', argument " "8"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
-  }
-  if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "8"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
-  }
-  arg8 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp8);
-  res9 = SWIG_ConvertPtr(argv[8], &argp9, SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers,  0 );
-  if (!SWIG_IsOK(res9)) {
-    SWIG_exception_fail(SWIG_ArgError(res9), "in method '" "estimateExternalWrenches" "', argument " "9"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
-  }
-  if (!argp9) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "9"" of type '" "iDynTree::estimateExternalWrenchesBuffers &""'"); 
-  }
-  arg9 = reinterpret_cast< iDynTree::estimateExternalWrenchesBuffers * >(argp9);
-  res10 = SWIG_ConvertPtr(argv[9], &argp10, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
-  if (!SWIG_IsOK(res10)) {
-    SWIG_exception_fail(SWIG_ArgError(res10), "in method '" "estimateExternalWrenches" "', argument " "10"" of type '" "iDynTree::LinkContactWrenches &""'"); 
-  }
-  if (!argp10) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateExternalWrenches" "', argument " "10"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getDynamicVariablesOrdering" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg10 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp10);
-  result = (bool)iDynTree::estimateExternalWrenches((iDynTree::Model const &)*arg1,(iDynTree::SubModelDecomposition const &)*arg2,(iDynTree::SensorsList const &)*arg3,(iDynTree::LinkUnknownWrenchContacts const &)*arg4,(iDynTree::JointPosDoubleArray const &)*arg5,(iDynTree::LinkVelArray const &)*arg6,(iDynTree::LinkAccArray const &)*arg7,(iDynTree::SensorsMeasurements const &)*arg8,*arg9,*arg10);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > *) &((iDynTree::BerdyHelper const *)arg1)->getDynamicVariablesOrdering();
+  _out = swig::from(static_cast< std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > >(*result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -71262,124 +77006,99 @@ int _wrap_estimateExternalWrenches(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_dynamicsEstimationForwardVelAccKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::Vector3 *arg3 = 0 ;
-  iDynTree::Vector3 *arg4 = 0 ;
-  iDynTree::Vector3 *arg5 = 0 ;
-  iDynTree::JointPosDoubleArray *arg6 = 0 ;
+int _wrap_BerdyHelper_serializeDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::LinkProperAccArray *arg2 = 0 ;
+  iDynTree::LinkNetTotalWrenchesWithoutGravity *arg3 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg4 = 0 ;
+  iDynTree::LinkInternalWrenches *arg5 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg6 = 0 ;
   iDynTree::JointDOFsDoubleArray *arg7 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg8 = 0 ;
-  iDynTree::LinkVelArray *arg9 = 0 ;
-  iDynTree::LinkAccArray *arg10 = 0 ;
-  void *argp1 ;
+  iDynTree::VectorDynSize *arg8 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 ;
+  void *argp3 = 0 ;
   int res3 = 0 ;
-  void *argp4 ;
+  void *argp4 = 0 ;
   int res4 = 0 ;
-  void *argp5 ;
+  void *argp5 = 0 ;
   int res5 = 0 ;
-  void *argp6 ;
+  void *argp6 = 0 ;
   int res6 = 0 ;
-  void *argp7 ;
-  int res7 = 0 ;
-  void *argp8 ;
-  int res8 = 0 ;
-  void *argp9 = 0 ;
-  int res9 = 0 ;
-  void *argp10 = 0 ;
-  int res10 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
+  void *argp8 = 0 ;
+  int res8 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("dynamicsEstimationForwardVelAccKinematics",argc,10,10,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_serializeDynamicVariables",argc,8,8,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "2"" of type '" "iDynTree::LinkProperAccArray &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "2"" of type '" "iDynTree::LinkProperAccArray &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  arg2 = reinterpret_cast< iDynTree::LinkProperAccArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Vector3 * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  arg3 = reinterpret_cast< iDynTree::LinkNetTotalWrenchesWithoutGravity * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "4"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "4"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::Vector3 * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  arg4 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "5"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "5"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  arg5 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "6"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "6"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "6"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp6);
+  arg6 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp6);
   res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   arg7 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "8"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "8"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "8"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg8 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp8);
-  res9 = SWIG_ConvertPtr(argv[8], &argp9, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
-  if (!SWIG_IsOK(res9)) {
-    SWIG_exception_fail(SWIG_ArgError(res9), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "9"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  if (!argp9) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "9"" of type '" "iDynTree::LinkVelArray &""'"); 
-  }
-  arg9 = reinterpret_cast< iDynTree::LinkVelArray * >(argp9);
-  res10 = SWIG_ConvertPtr(argv[9], &argp10, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res10)) {
-    SWIG_exception_fail(SWIG_ArgError(res10), "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "10"" of type '" "iDynTree::LinkAccArray &""'"); 
-  }
-  if (!argp10) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelAccKinematics" "', argument " "10"" of type '" "iDynTree::LinkAccArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "8"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
-  arg10 = reinterpret_cast< iDynTree::LinkAccArray * >(argp10);
-  result = (bool)iDynTree::dynamicsEstimationForwardVelAccKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::VectorFixSize< 3 > const &)*arg3,(iDynTree::VectorFixSize< 3 > const &)*arg4,(iDynTree::VectorFixSize< 3 > const &)*arg5,(iDynTree::JointPosDoubleArray const &)*arg6,(iDynTree::JointDOFsDoubleArray const &)*arg7,(iDynTree::JointDOFsDoubleArray const &)*arg8,*arg9,*arg10);
+  arg8 = reinterpret_cast< iDynTree::VectorDynSize * >(argp8);
+  result = (bool)(arg1)->serializeDynamicVariables(*arg2,*arg3,*arg4,*arg5,*arg6,*arg7,*arg8);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -71388,80 +77107,88 @@ int _wrap_dynamicsEstimationForwardVelAccKinematics(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_dynamicsEstimationForwardVelKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::Traversal *arg2 = 0 ;
-  iDynTree::Vector3 *arg3 = 0 ;
-  iDynTree::JointPosDoubleArray *arg4 = 0 ;
+int _wrap_BerdyHelper_serializeSensorVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::SensorsMeasurements *arg2 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg4 = 0 ;
   iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
-  iDynTree::LinkVelArray *arg6 = 0 ;
-  void *argp1 ;
+  iDynTree::LinkInternalWrenches *arg6 = 0 ;
+  iDynTree::VectorDynSize *arg7 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 ;
+  void *argp3 = 0 ;
   int res3 = 0 ;
-  void *argp4 ;
+  void *argp4 = 0 ;
   int res4 = 0 ;
-  void *argp5 ;
+  void *argp5 = 0 ;
   int res5 = 0 ;
   void *argp6 = 0 ;
   int res6 = 0 ;
+  void *argp7 = 0 ;
+  int res7 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("dynamicsEstimationForwardVelKinematics",argc,6,6,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_serializeSensorVariables",argc,7,7,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Traversal,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "2"" of type '" "iDynTree::SensorsMeasurements &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "2"" of type '" "iDynTree::Traversal const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "2"" of type '" "iDynTree::SensorsMeasurements &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Traversal * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  arg2 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Vector3 * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "4"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp4);
+  arg4 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp4);
   res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
   }
   if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "dynamicsEstimationForwardVelKinematics" "', argument " "6"" of type '" "iDynTree::LinkVelArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkVelArray * >(argp6);
-  result = (bool)iDynTree::dynamicsEstimationForwardVelKinematics((iDynTree::Model const &)*arg1,(iDynTree::Traversal const &)*arg2,(iDynTree::VectorFixSize< 3 > const &)*arg3,(iDynTree::JointPosDoubleArray const &)*arg4,(iDynTree::JointDOFsDoubleArray const &)*arg5,*arg6);
+  arg6 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "7"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "7"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::VectorDynSize * >(argp7);
+  result = (bool)(arg1)->serializeSensorVariables(*arg2,*arg3,*arg4,*arg5,*arg6,*arg7);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -71470,58 +77197,55 @@ int _wrap_dynamicsEstimationForwardVelKinematics(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_computeLinkNetWrenchesWithoutGravity(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::LinkVelArray *arg2 = 0 ;
-  iDynTree::LinkAccArray *arg3 = 0 ;
-  iDynTree::LinkNetTotalWrenchesWithoutGravity *arg4 = 0 ;
-  void *argp1 ;
+int _wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::JointDOFsDoubleArray *arg2 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 ;
+  void *argp3 = 0 ;
   int res3 = 0 ;
   void *argp4 = 0 ;
   int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("computeLinkNetWrenchesWithoutGravity",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkVelArray,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkVelArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkVelArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
+  arg2 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "3"" of type '" "iDynTree::LinkAccArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "3"" of type '" "iDynTree::LinkAccArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::LinkAccArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "4"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "computeLinkNetWrenchesWithoutGravity" "', argument " "4"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkNetTotalWrenchesWithoutGravity * >(argp4);
-  result = (bool)iDynTree::computeLinkNetWrenchesWithoutGravity((iDynTree::Model const &)*arg1,(iDynTree::LinkVelArray const &)*arg2,(iDynTree::LinkAccArray const &)*arg3,*arg4);
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  result = (bool)(arg1)->serializeDynamicVariablesComputedFromFixedBaseRNEA(*arg2,*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -71530,12 +77254,12 @@ int _wrap_computeLinkNetWrenchesWithoutGravity(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::Model *arg1 = 0 ;
-  iDynTree::LinkUnknownWrenchContacts *arg2 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
-  iDynTree::LinkContactWrenches *arg4 = 0 ;
-  void *argp1 ;
+int _wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
@@ -71546,42 +77270,39 @@ int _wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(int resc, mxArr
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("estimateLinkContactWrenchesFromLinkNetExternalWrenches",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_extractJointTorquesFromDynamicVariables",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__Model,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "1"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
   if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateLinkContactWrenchesFromLinkNetExternalWrenches" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp4);
-  result = (bool)iDynTree::estimateLinkContactWrenchesFromLinkNetExternalWrenches((iDynTree::Model const &)*arg1,(iDynTree::LinkUnknownWrenchContacts const &)*arg2,(iDynTree::LinkWrenches const &)*arg3,*arg4);
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  result = (bool)((iDynTree::BerdyHelper const *)arg1)->extractJointTorquesFromDynamicVariables((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -71590,88 +77311,44 @@ int _wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(int resc, mxArr
 }
 
 
-int _wrap_new_ExtWrenchesAndJointTorquesEstimator(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_ExtWrenchesAndJointTorquesEstimator",argc,0,0,0)) {
-    SWIG_fail;
-  }
-  (void)argv;
-  result = (iDynTree::ExtWrenchesAndJointTorquesEstimator *)new iDynTree::ExtWrenchesAndJointTorquesEstimator();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_delete_ExtWrenchesAndJointTorquesEstimator(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  
-  int is_owned;
-  if (!SWIG_check_num_args("delete_ExtWrenchesAndJointTorquesEstimator",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, SWIG_POINTER_DISOWN |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ExtWrenchesAndJointTorquesEstimator" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  if (is_owned) {
-    delete arg1;
-  }
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::SensorsList *arg3 = 0 ;
+int _wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
+  void *argp3 = 0 ;
   int res3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_setModelAndSensors",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
-  result = (bool)(arg1)->setModelAndSensors((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3);
+  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
+  result = (bool)((iDynTree::BerdyHelper const *)arg1)->extractLinkNetExternalWrenchesFromDynamicVariables((iDynTree::VectorDynSize const &)*arg2,*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -71680,42 +77357,65 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(int resc, mxArr
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  std::string arg2 ;
-  std::string arg3 ;
+int _wrap_BerdyHelper_updateKinematicsFromFloatingBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::FrameIndex *arg4 = 0 ;
+  iDynTree::Vector3 *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  iDynTree::FrameIndex temp4 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_updateKinematicsFromFloatingBase",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "3"" of type '" "std::string const""'"); 
-    }
-    arg3 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  result = (bool)(arg1)->loadModelAndSensorsFromFile(arg2,arg3);
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  temp4 = static_cast< iDynTree::FrameIndex >(val4);
+  arg4 = &temp4;
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
+  result = (bool)(arg1)->updateKinematicsFromFloatingBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::FrameIndex const &)*arg4,(iDynTree::Vector3 const &)*arg5);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -71724,253 +77424,236 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  std::string arg2 ;
+int _wrap_BerdyHelper_updateKinematicsFromFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::FrameIndex *arg4 = 0 ;
+  iDynTree::Vector3 *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  iDynTree::FrameIndex temp4 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_updateKinematicsFromFixedBase",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  result = (bool)(arg1)->loadModelAndSensorsFromFile(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_1(resc,resv,argc,argv);
-      }
-    }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  temp4 = static_cast< iDynTree::FrameIndex >(val4);
+  arg4 = &temp4;
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFile(std::string const,std::string const)\n"
-    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFile(std::string const)\n");
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
+  result = (bool)(arg1)->updateKinematicsFromFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::FrameIndex const &)*arg4,(iDynTree::Vector3 const &)*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
-  std::string arg4 ;
+int _wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::Vector3 *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_updateKinematicsFromTraversalFixedBase",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "4"" of type '" "std::string const""'"); 
-    }
-    arg4 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
   }
-  result = (bool)(arg1)->loadModelAndSensorsFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Vector3 * >(argp4);
+  result = (bool)(arg1)->updateKinematicsFromTraversalFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::Vector3 const &)*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+int _wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::Transform *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdyHelper_setNetExternalWrenchMeasurementFrame",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
   }
-  result = (bool)(arg1)->loadModelAndSensorsFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  result = (bool)(arg1)->setNetExternalWrenchMeasurementFrame(arg2,(iDynTree::Transform const &)*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
+int _wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  iDynTree::Transform *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("BerdyHelper_getNetExternalWrenchMeasurementFrame",argc,3,3,0)) {
+    SWIG_fail;
   }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
-    "    iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  result = (bool)((iDynTree::BerdyHelper const *)arg1)->getNetExternalWrenchMeasurementFrame(arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+int _wrap_delete_BerdyHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_model",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_BerdyHelper",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_model" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdyHelper" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  result = (iDynTree::Model *) &((iDynTree::ExtWrenchesAndJointTorquesEstimator const *)arg1)->model();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -71978,23 +77661,26 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_model(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+int _wrap_new_BerdySparseMAPSolver(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdyHelper *arg1 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SensorsList *result = 0 ;
+  iDynTree::BerdySparseMAPSolver *result = 0 ;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_sensors",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_BerdySparseMAPSolver",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__BerdyHelper,  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_sensors" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_BerdySparseMAPSolver" "', argument " "1"" of type '" "iDynTree::BerdyHelper &""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  result = (iDynTree::SensorsList *) &((iDynTree::ExtWrenchesAndJointTorquesEstimator const *)arg1)->sensors();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_BerdySparseMAPSolver" "', argument " "1"" of type '" "iDynTree::BerdyHelper &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
+  result = (iDynTree::BerdySparseMAPSolver *)new iDynTree::BerdySparseMAPSolver(*arg1);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72002,23 +77688,26 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_sensors(int resc, mxArray *resv[],
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_submodels(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
+int _wrap_delete_BerdySparseMAPSolver(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SubModelDecomposition *result = 0 ;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_submodels",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_BerdySparseMAPSolver",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_submodels" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdySparseMAPSolver" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  result = (iDynTree::SubModelDecomposition *) &((iDynTree::ExtWrenchesAndJointTorquesEstimator const *)arg1)->submodels();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SubModelDecomposition, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72026,99 +77715,33 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_submodels(int resc, mxArray *resv[
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg4 = 0 ;
-  iDynTree::FrameIndex *arg5 = 0 ;
-  iDynTree::Vector3 *arg6 = 0 ;
-  iDynTree::Vector3 *arg7 = 0 ;
-  iDynTree::Vector3 *arg8 = 0 ;
+int _wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  iDynTree::FrameIndex temp5 ;
-  int val5 ;
-  int ecode5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
-  void *argp7 ;
-  int res7 = 0 ;
-  void *argp8 ;
-  int res8 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase",argc,8,8,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "5"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  temp5 = static_cast< iDynTree::FrameIndex >(val5);
-  arg5 = &temp5;
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::Vector3 * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "7"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "7"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::Vector3 * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "8"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase" "', argument " "8"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
   }
-  arg8 = reinterpret_cast< iDynTree::Vector3 * >(argp8);
-  result = (bool)(arg1)->updateKinematicsFromFloatingBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::JointDOFsDoubleArray const &)*arg4,(iDynTree::FrameIndex const &)*arg5,(iDynTree::Vector3 const &)*arg6,(iDynTree::Vector3 const &)*arg7,(iDynTree::Vector3 const &)*arg8);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
+  (arg1)->setDynamicsConstraintsPriorCovariance((iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72126,77 +77749,33 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(i
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg4 = 0 ;
-  iDynTree::FrameIndex *arg5 = 0 ;
-  iDynTree::Vector3 *arg6 = 0 ;
+int _wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
-  iDynTree::FrameIndex temp5 ;
-  int val5 ;
-  int ecode5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase",argc,6,6,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp4);
-  ecode5 = SWIG_AsVal_int(argv[4], &val5);
-  if (!SWIG_IsOK(ecode5)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "5"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  temp5 = static_cast< iDynTree::FrameIndex >(val5);
-  arg5 = &temp5;
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase" "', argument " "6"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::Vector3 * >(argp6);
-  result = (bool)(arg1)->updateKinematicsFromFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::JointDOFsDoubleArray const &)*arg4,(iDynTree::FrameIndex const &)*arg5,(iDynTree::Vector3 const &)*arg6);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
+  (arg1)->setDynamicsRegularizationPriorCovariance((iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72204,67 +77783,33 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(int
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  iDynTree::LinkUnknownWrenchContacts *arg2 = 0 ;
-  iDynTree::SensorsMeasurements *arg3 = 0 ;
-  iDynTree::LinkContactWrenches *arg4 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
+int _wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements",argc,5,5,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
-  result = (bool)(arg1)->computeExpectedFTSensorsMeasurements((iDynTree::LinkUnknownWrenchContacts const &)*arg2,*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  (arg1)->setDynamicsRegularizationPriorExpectedValue((iDynTree::VectorDynSize const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72272,67 +77817,33 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasuremen
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  iDynTree::LinkUnknownWrenchContacts *arg2 = 0 ;
-  iDynTree::SensorsMeasurements *arg3 = 0 ;
-  iDynTree::LinkContactWrenches *arg4 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
+int _wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques",argc,5,5,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_setMeasurementsPriorCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setMeasurementsPriorCovariance" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkUnknownWrenchContacts,  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setMeasurementsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "2"" of type '" "iDynTree::LinkUnknownWrenchContacts const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::LinkUnknownWrenchContacts * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "3"" of type '" "iDynTree::SensorsMeasurements const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkContactWrenches,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "4"" of type '" "iDynTree::LinkContactWrenches &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkContactWrenches * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setMeasurementsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
   }
-  arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
-  result = (bool)(arg1)->estimateExtWrenchesAndJointTorques((iDynTree::LinkUnknownWrenchContacts const &)*arg2,(iDynTree::SensorsMeasurements const &)*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
+  (arg1)->setMeasurementsPriorCovariance((iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72340,47 +77851,23 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  double arg2 ;
-  double arg3 ;
-  double arg4 ;
+int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double val2 ;
-  int ecode2 = 0 ;
-  double val3 ;
-  int ecode3 = 0 ;
-  double val4 ;
-  int ecode4 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  ecode2 = SWIG_AsVal_double(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "2"" of type '" "double""'");
-  } 
-  arg2 = static_cast< double >(val2);
-  ecode3 = SWIG_AsVal_double(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "3"" of type '" "double""'");
-  } 
-  arg3 = static_cast< double >(val3);
-  ecode4 = SWIG_AsVal_double(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill" "', argument " "4"" of type '" "double""'");
-  } 
-  arg4 = static_cast< double >(val4);
-  result = (bool)(arg1)->checkThatTheModelIsStill(arg2,arg3,arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->dynamicsConstraintsPriorCovarianceInverse();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72388,34 +77875,23 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(int resc,
 }
 
 
-int _wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::ExtWrenchesAndJointTorquesEstimator *arg1 = (iDynTree::ExtWrenchesAndJointTorquesEstimator *) 0 ;
-  iDynTree::LinkNetTotalWrenchesWithoutGravity *arg2 = 0 ;
+int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  if (!SWIG_check_num_args("ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity" "', argument " "1"" of type '" "iDynTree::ExtWrenchesAndJointTorquesEstimator *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::ExtWrenchesAndJointTorquesEstimator * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity" "', argument " "2"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::LinkNetTotalWrenchesWithoutGravity * >(argp2);
-  result = (bool)(arg1)->estimateLinkNetWrenchesWithoutGravity(*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &(arg1)->dynamicsConstraintsPriorCovarianceInverse();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72423,43 +77899,51 @@ int _wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGrav
 }
 
 
-int _wrap_new_SimpleLeggedOdometry(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::SimpleLeggedOdometry *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_SimpleLeggedOdometry",argc,0,0,0)) {
-    SWIG_fail;
+int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_1(resc,resv,argc,argv);
+    }
   }
-  (void)argv;
-  result = (iDynTree::SimpleLeggedOdometry *)new iDynTree::SimpleLeggedOdometry();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::BerdySparseMAPSolver::dynamicsConstraintsPriorCovarianceInverse() const\n"
+    "    iDynTree::BerdySparseMAPSolver::dynamicsConstraintsPriorCovarianceInverse()\n");
   return 1;
 }
 
 
-int _wrap_delete_SimpleLeggedOdometry(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_SimpleLeggedOdometry",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse",argc,1,1,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_SimpleLeggedOdometry" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
   }
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->dynamicsRegularizationPriorCovarianceInverse();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72467,34 +77951,23 @@ int _wrap_delete_SimpleLeggedOdometry(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_SimpleLeggedOdometry_setModel(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
+int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_setModel",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_setModel" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_setModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_setModel" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  result = (bool)(arg1)->setModel((iDynTree::Model const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &(arg1)->dynamicsRegularizationPriorCovarianceInverse();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72502,43 +77975,51 @@ int _wrap_SimpleLeggedOdometry_setModel(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string arg2 ;
-  std::string arg3 ;
+int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_1(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorCovarianceInverse() const\n"
+    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorCovarianceInverse()\n");
+  return 1;
+}
+
+
+int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::VectorDynSize *result = 0 ;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFile",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "3"" of type '" "std::string const""'"); 
-    }
-    arg3 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
   }
-  result = (bool)(arg1)->loadModelFromFile(arg2,arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::VectorDynSize *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->dynamicsRegularizationPriorExpectedValue();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorDynSize, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72546,33 +78027,23 @@ int _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_0(int resc, mxArray *resv
 }
 
 
-int _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string arg2 ;
+int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::VectorDynSize *result = 0 ;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFile",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFile" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  result = (bool)(arg1)->loadModelFromFile(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::VectorDynSize *) &(arg1)->dynamicsRegularizationPriorExpectedValue();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorDynSize, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72580,219 +78051,151 @@ int _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_1(int resc, mxArray *resv
 }
 
 
-int _wrap_SimpleLeggedOdometry_loadModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
+int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_1(resc,resv,argc,argv);
-      }
+      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_1(resc,resv,argc,argv);
     }
   }
-  if (argc == 3) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_SimpleLeggedOdometry_loadModelFromFile__SWIG_0(resc,resv,argc,argv);
-        }
-      }
+      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_loadModelFromFile'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SimpleLeggedOdometry::loadModelFromFile(std::string const,std::string const)\n"
-    "    iDynTree::SimpleLeggedOdometry::loadModelFromFile(std::string const)\n");
+    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorExpectedValue() const\n"
+    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorExpectedValue()\n");
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
-  std::string arg4 ;
+int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
-  bool result;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs",argc,4,4,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_measurementsPriorCovarianceInverse",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "4"" of type '" "std::string const""'"); 
-    }
-    arg4 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_measurementsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
   }
-  result = (bool)(arg1)->loadModelFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3,arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->measurementsPriorCovarianceInverse();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string arg2 ;
-  std::vector< std::string,std::allocator< std::string > > *arg3 = 0 ;
+int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
-  bool result;
+  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_measurementsPriorCovarianceInverse",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    int res = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res) || !ptr) {
-      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "2"" of type '" "std::string const""'"); 
-    }
-    arg2 = *ptr;
-    if (SWIG_IsNewObj(res)) delete ptr;
-  }
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res3 = swig::asptr(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs" "', argument " "3"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg3 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_measurementsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  result = (bool)(arg1)->loadModelFromFileWithSpecifiedDOFs(arg2,(std::vector< std::string,std::allocator< std::string > > const &)*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &(arg1)->measurementsPriorCovarianceInverse();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
+int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_1(resc,resv,argc,argv);
-        }
-      }
+      return _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_1(resc,resv,argc,argv);
     }
   }
-  if (argc == 4) {
+  if (argc == 1) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        int res = swig::asptr(argv[2], (std::vector< std::string,std::allocator< std::string > >**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
+      return _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_0(resc,resv,argc,argv);
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_measurementsPriorCovarianceInverse'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SimpleLeggedOdometry::loadModelFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &,std::string const)\n"
-    "    iDynTree::SimpleLeggedOdometry::loadModelFromFileWithSpecifiedDOFs(std::string const,std::vector< std::string,std::allocator< std::string > > const &)\n");
+    "    iDynTree::BerdySparseMAPSolver::measurementsPriorCovarianceInverse() const\n"
+    "    iDynTree::BerdySparseMAPSolver::measurementsPriorCovarianceInverse()\n");
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+int _wrap_BerdySparseMAPSolver_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_model",argc,1,1,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_isValid",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_isValid" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (bool)(arg1)->isValid();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_BerdySparseMAPSolver_initialize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_initialize",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_model" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_initialize" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  result = (iDynTree::Model *) &((iDynTree::SimpleLeggedOdometry const *)arg1)->model();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (bool)(arg1)->initialize();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72800,34 +78203,74 @@ int _wrap_SimpleLeggedOdometry_model(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_SimpleLeggedOdometry_updateKinematics(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+int _wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::FrameIndex arg4 ;
+  iDynTree::Vector3 *arg5 = 0 ;
+  iDynTree::VectorDynSize *arg6 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_updateKinematics",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_updateEstimateInformationFixedBase",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_updateKinematics" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
   res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_updateKinematics" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_updateKinematics" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
   arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  result = (bool)(arg1)->updateKinematics(*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::FrameIndex >(val4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
+  (arg1)->updateEstimateInformationFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,arg4,(iDynTree::Vector3 const &)*arg5,(iDynTree::VectorDynSize const &)*arg6);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72835,137 +78278,132 @@ int _wrap_SimpleLeggedOdometry_updateKinematics(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string *arg2 = 0 ;
-  iDynTree::Transform arg3 ;
+int _wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  iDynTree::JointPosDoubleArray *arg2 = 0 ;
+  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
+  iDynTree::FrameIndex arg4 ;
+  iDynTree::Vector3 *arg5 = 0 ;
+  iDynTree::VectorDynSize *arg6 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  void *argp2 ;
+  int res2 = 0 ;
   void *argp3 ;
   int res3 = 0 ;
+  int val4 ;
+  int ecode4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_updateEstimateInformationFloatingBase",argc,6,6,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  {
-    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'"); 
-    }  
-    if (!argp3) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'");
-    } else {
-      arg3 = *(reinterpret_cast< iDynTree::Transform * >(argp3));
-    }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
   }
-  result = (bool)(arg1)->init((std::string const &)*arg2,arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
+  ecode4 = SWIG_AsVal_int(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg4 = static_cast< iDynTree::FrameIndex >(val4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
+  (arg1)->updateEstimateInformationFloatingBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,arg4,(iDynTree::Vector3 const &)*arg5,(iDynTree::VectorDynSize const &)*arg6);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_BerdySparseMAPSolver_doEstimate(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_doEstimate",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_doEstimate" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
   }
-  result = (bool)(arg1)->init((std::string const &)*arg2);
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (bool)(arg1)->doEstimate();
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::FrameIndex arg2 ;
-  iDynTree::Transform arg3 ;
+int _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_getLastEstimate",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  {
-    res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'"); 
-    }  
-    if (!argp3) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::Transform const""'");
-    } else {
-      arg3 = *(reinterpret_cast< iDynTree::Transform * >(argp3));
-    }
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
-  result = (bool)(arg1)->init(arg2,arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  ((iDynTree::BerdySparseMAPSolver const *)arg1)->getLastEstimate(*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -72973,31 +78411,23 @@ int _wrap_SimpleLeggedOdometry_init__SWIG_2(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::FrameIndex arg2 ;
+int _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::VectorDynSize *result = 0 ;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,2,2,0)) {
+  if (!SWIG_check_num_args("BerdySparseMAPSolver_getLastEstimate",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  result = (bool)(arg1)->init(arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  result = (iDynTree::VectorDynSize *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->getLastEstimate();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorDynSize, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73005,174 +78435,104 @@ int _wrap_SimpleLeggedOdometry_init__SWIG_3(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_4(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
-  iDynTree::Transform arg4 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
-  void *argp4 ;
-  int res4 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,4,4,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
-  }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
+int _wrap_BerdySparseMAPSolver_getLastEstimate(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_1(resc,resv,argc,argv);
     }
-    arg3 = ptr;
   }
-  {
-    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
-    if (!SWIG_IsOK(res4)) {
-      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'"); 
-    }  
-    if (!argp4) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'");
-    } else {
-      arg4 = *(reinterpret_cast< iDynTree::Transform * >(argp4));
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_0(resc,resv,argc,argv);
+      }
     }
   }
-  result = (bool)(arg1)->init((std::string const &)*arg2,(std::string const &)*arg3,arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
-  return 0;
-fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_getLastEstimate'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::BerdySparseMAPSolver::getLastEstimate(iDynTree::VectorDynSize &) const\n"
+    "    iDynTree::BerdySparseMAPSolver::getLastEstimate() const\n");
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_5(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string *arg2 = 0 ;
-  std::string *arg3 = 0 ;
+int _wrap_delete_IAttitudeEstimator(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
-  int res3 = SWIG_OLDOBJ ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_IAttitudeEstimator",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_IAttitudeEstimator" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
   }
-  {
-    std::string *ptr = (std::string *)0;
-    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
-    if (!SWIG_IsOK(res3)) {
-      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "std::string const &""'"); 
-    }
-    arg3 = ptr;
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  result = (bool)(arg1)->init((std::string const &)*arg2,(std::string const &)*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
-  if (SWIG_IsNewObj(res3)) delete arg3;
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_6(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::FrameIndex arg2 ;
-  iDynTree::FrameIndex arg3 ;
-  iDynTree::Transform arg4 ;
+int _wrap_IAttitudeEstimator_updateFilterWithMeasurements__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::LinearAccelerometerMeasurements *arg2 = 0 ;
+  iDynTree::GyroscopeMeasurements *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,4,4,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_updateFilterWithMeasurements",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::FrameIndex >(val3);
-  {
-    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Transform,  0 );
-    if (!SWIG_IsOK(res4)) {
-      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'"); 
-    }  
-    if (!argp4) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_init" "', argument " "4"" of type '" "iDynTree::Transform const""'");
-    } else {
-      arg4 = *(reinterpret_cast< iDynTree::Transform * >(argp4));
-    }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
   }
-  result = (bool)(arg1)->init(arg2,arg3,arg4);
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinearAccelerometerMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::GyroscopeMeasurements * >(argp3);
+  result = (bool)(arg1)->updateFilterWithMeasurements((iDynTree::LinearAccelerometerMeasurements const &)*arg2,(iDynTree::GyroscopeMeasurements const &)*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73181,173 +78541,79 @@ int _wrap_SimpleLeggedOdometry_init__SWIG_6(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_SimpleLeggedOdometry_init__SWIG_7(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::FrameIndex arg2 ;
-  iDynTree::FrameIndex arg3 ;
+int _wrap_IAttitudeEstimator_updateFilterWithMeasurements__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::LinearAccelerometerMeasurements *arg2 = 0 ;
+  iDynTree::GyroscopeMeasurements *arg3 = 0 ;
+  iDynTree::MagnetometerMeasurements *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_init",argc,3,3,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_updateFilterWithMeasurements",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_init" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_init" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "SimpleLeggedOdometry_init" "', argument " "3"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::FrameIndex >(val3);
-  result = (bool)(arg1)->init(arg2,arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_SimpleLeggedOdometry_init(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_SimpleLeggedOdometry_init__SWIG_3(resc,resv,argc,argv);
-      }
-    }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
   }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_SimpleLeggedOdometry_init__SWIG_1(resc,resv,argc,argv);
-      }
-    }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinearAccelerometerMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
   }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_SimpleLeggedOdometry_init__SWIG_2(resc,resv,argc,argv);
-        }
-      }
-    }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
   }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
-        if (_v) {
-          return _wrap_SimpleLeggedOdometry_init__SWIG_7(resc,resv,argc,argv);
-        }
-      }
-    }
+  arg3 = reinterpret_cast< iDynTree::GyroscopeMeasurements * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "4"" of type '" "iDynTree::MagnetometerMeasurements const &""'"); 
   }
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_SimpleLeggedOdometry_init__SWIG_0(resc,resv,argc,argv);
-        }
-      }
-    }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_updateFilterWithMeasurements" "', argument " "4"" of type '" "iDynTree::MagnetometerMeasurements const &""'"); 
   }
+  arg4 = reinterpret_cast< iDynTree::MagnetometerMeasurements * >(argp4);
+  result = (bool)(arg1)->updateFilterWithMeasurements((iDynTree::LinearAccelerometerMeasurements const &)*arg2,(iDynTree::GyroscopeMeasurements const &)*arg3,(iDynTree::MagnetometerMeasurements const &)*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IAttitudeEstimator_updateFilterWithMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_SimpleLeggedOdometry_init__SWIG_5(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
         _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        {
-          int res = SWIG_AsVal_int(argv[2], NULL);
-          _v = SWIG_CheckState(res);
-        }
         if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_SimpleLeggedOdometry_init__SWIG_6(resc,resv,argc,argv);
-          }
+          return _wrap_IAttitudeEstimator_updateFilterWithMeasurements__SWIG_0(resc,resv,argc,argv);
         }
       }
     }
@@ -73355,103 +78621,87 @@ int _wrap_SimpleLeggedOdometry_init(int resc, mxArray *resv[], int argc, mxArray
   if (argc == 4) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
       _v = SWIG_CheckState(res);
       if (_v) {
-        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
         _v = SWIG_CheckState(res);
         if (_v) {
           void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
           _v = SWIG_CheckState(res);
           if (_v) {
-            return _wrap_SimpleLeggedOdometry_init__SWIG_4(resc,resv,argc,argv);
+            return _wrap_IAttitudeEstimator_updateFilterWithMeasurements__SWIG_1(resc,resv,argc,argv);
           }
         }
       }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_init'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'IAttitudeEstimator_updateFilterWithMeasurements'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SimpleLeggedOdometry::init(std::string const &,iDynTree::Transform const)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(std::string const &)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const,iDynTree::Transform const)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(std::string const &,std::string const &,iDynTree::Transform const)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(std::string const &,std::string const &)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const,iDynTree::FrameIndex const,iDynTree::Transform const)\n"
-    "    iDynTree::SimpleLeggedOdometry::init(iDynTree::FrameIndex const,iDynTree::FrameIndex const)\n");
+    "    iDynTree::IAttitudeEstimator::updateFilterWithMeasurements(iDynTree::LinearAccelerometerMeasurements const &,iDynTree::GyroscopeMeasurements const &)\n"
+    "    iDynTree::IAttitudeEstimator::updateFilterWithMeasurements(iDynTree::LinearAccelerometerMeasurements const &,iDynTree::GyroscopeMeasurements const &,iDynTree::MagnetometerMeasurements const &)\n");
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_IAttitudeEstimator_propagateStates(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_changeFixedFrame",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_propagateStates",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_propagateStates" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
   }
-  result = (bool)(arg1)->changeFixedFrame((std::string const &)*arg2);
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  result = (bool)(arg1)->propagateStates();
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::FrameIndex arg2 ;
+int _wrap_IAttitudeEstimator_getOrientationEstimateAsRotationMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::Rotation *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_changeFixedFrame",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_getOrientationEstimateAsRotationMatrix",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::FrameIndex >(val2);
-  result = (bool)(arg1)->changeFixedFrame(arg2);
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Rotation,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix" "', argument " "2"" of type '" "iDynTree::Rotation &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix" "', argument " "2"" of type '" "iDynTree::Rotation &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Rotation * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsRotationMatrix(*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73460,61 +78710,93 @@ int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_1(int resc, mxArray *resv[
 }
 
 
-int _wrap_SimpleLeggedOdometry_changeFixedFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      {
-        int res = SWIG_AsVal_int(argv[1], NULL);
-        _v = SWIG_CheckState(res);
-      }
-      if (_v) {
-        return _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_1(resc,resv,argc,argv);
-      }
-    }
+int _wrap_IAttitudeEstimator_getOrientationEstimateAsQuaternion(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::UnitQuaternion *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IAttitudeEstimator_getOrientationEstimateAsQuaternion",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_0(resc,resv,argc,argv);
-      }
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_getOrientationEstimateAsQuaternion" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_getOrientationEstimateAsQuaternion" "', argument " "2"" of type '" "iDynTree::UnitQuaternion &""'"); 
   }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_getOrientationEstimateAsQuaternion" "', argument " "2"" of type '" "iDynTree::UnitQuaternion &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::UnitQuaternion * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsQuaternion(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IAttitudeEstimator_getOrientationEstimateAsRPY(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::RPY *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_changeFixedFrame'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(std::string const &)\n"
-    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(iDynTree::FrameIndex const)\n");
+  if (!SWIG_check_num_args("IAttitudeEstimator_getOrientationEstimateAsRPY",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_getOrientationEstimateAsRPY" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_getOrientationEstimateAsRPY" "', argument " "2"" of type '" "iDynTree::RPY &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_getOrientationEstimateAsRPY" "', argument " "2"" of type '" "iDynTree::RPY &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::RPY * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsRPY(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_SimpleLeggedOdometry_getCurrentFixedLink(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+int _wrap_IAttitudeEstimator_getInternalStateSize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  std::string result;
+  size_t result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_getCurrentFixedLink",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_getInternalStateSize",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_getCurrentFixedLink" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_getInternalStateSize" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  result = (arg1)->getCurrentFixedLink();
-  _out = SWIG_From_std_string(static_cast< std::string >(result));
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  result = ((iDynTree::IAttitudeEstimator const *)arg1)->getInternalStateSize();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73522,31 +78804,34 @@ int _wrap_SimpleLeggedOdometry_getCurrentFixedLink(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_SimpleLeggedOdometry_getWorldLinkTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
-  iDynTree::LinkIndex arg2 ;
+int _wrap_IAttitudeEstimator_getInternalState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Transform result;
+  bool result;
   
-  if (!SWIG_check_num_args("SimpleLeggedOdometry_getWorldLinkTransform",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_getInternalState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_getWorldLinkTransform" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_getInternalState" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_getWorldLinkTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  result = (arg1)->getWorldLinkTransform(arg2);
-  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_getInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_getInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::IAttitudeEstimator const *)arg1)->getInternalState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73554,22 +78839,33 @@ int _wrap_SimpleLeggedOdometry_getWorldLinkTransform(int resc, mxArray *resv[],
 }
 
 
-int _wrap_isLinkBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariablesTypes arg1 ;
-  int val1 ;
-  int ecode1 = 0 ;
+int _wrap_IAttitudeEstimator_getDefaultInternalInitialState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("isLinkBerdyDynamicVariable",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_getDefaultInternalInitialState",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "isLinkBerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg1 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val1);
-  result = (bool)iDynTree::isLinkBerdyDynamicVariable(arg1);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_getDefaultInternalInitialState" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_getDefaultInternalInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_getDefaultInternalInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::IAttitudeEstimator const *)arg1)->getDefaultInternalInitialState((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73578,22 +78874,33 @@ int _wrap_isLinkBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArra
 }
 
 
-int _wrap_isJointBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariablesTypes arg1 ;
-  int val1 ;
-  int ecode1 = 0 ;
+int _wrap_IAttitudeEstimator_setInternalState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("isJointBerdyDynamicVariable",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_setInternalState",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "isJointBerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg1 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val1);
-  result = (bool)iDynTree::isJointBerdyDynamicVariable(arg1);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_setInternalState" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_setInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_setInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->setInternalState((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73602,22 +78909,33 @@ int _wrap_isJointBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_isDOFBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariablesTypes arg1 ;
-  int val1 ;
-  int ecode1 = 0 ;
+int _wrap_IAttitudeEstimator_setInternalStateInitialOrientation(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("isDOFBerdyDynamicVariable",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IAttitudeEstimator_setInternalStateInitialOrientation",argc,2,2,0)) {
     SWIG_fail;
   }
-  ecode1 = SWIG_AsVal_int(argv[0], &val1);
-  if (!SWIG_IsOK(ecode1)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "isDOFBerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg1 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val1);
-  result = (bool)iDynTree::isDOFBerdyDynamicVariable(arg1);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IAttitudeEstimator, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IAttitudeEstimator_setInternalStateInitialOrientation" "', argument " "1"" of type '" "iDynTree::IAttitudeEstimator *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IAttitudeEstimator * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IAttitudeEstimator_setInternalStateInitialOrientation" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IAttitudeEstimator_setInternalStateInitialOrientation" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->setInternalStateInitialOrientation((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73626,16 +78944,30 @@ int _wrap_isDOFBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_new_BerdyOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  double arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::BerdyOptions *result = 0 ;
   
-  if (!SWIG_check_num_args("new_BerdyOptions",argc,0,0,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_time_step_in_seconds_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::BerdyOptions *)new iDynTree::BerdyOptions();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdyOptions, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_time_step_in_seconds_set" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilterParameters_time_step_in_seconds_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->time_step_in_seconds = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73643,29 +78975,53 @@ int _wrap_new_BerdyOptions(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_BerdyOptions_berdyVariant_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  iDynTree::BerdyVariants arg2 ;
+int _wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  mxArray * _out;
+  double result;
+  
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_time_step_in_seconds_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_time_step_in_seconds_get" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  result = (double) ((arg1)->time_step_in_seconds);
+  _out = SWIG_From_double(static_cast< double >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeMahonyFilterParameters_kp_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  double arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdyOptions_berdyVariant_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_kp_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_berdyVariant_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_kp_set" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_berdyVariant_set" "', argument " "2"" of type '" "iDynTree::BerdyVariants""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilterParameters_kp_set" "', argument " "2"" of type '" "double""'");
   } 
-  arg2 = static_cast< iDynTree::BerdyVariants >(val2);
-  if (arg1) (arg1)->berdyVariant = arg2;
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->kp = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73674,23 +79030,23 @@ int _wrap_BerdyOptions_berdyVariant_set(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_BerdyOptions_berdyVariant_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilterParameters_kp_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::BerdyVariants result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyOptions_berdyVariant_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_kp_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_berdyVariant_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_kp_get" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (iDynTree::BerdyVariants) ((arg1)->berdyVariant);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  result = (double) ((arg1)->kp);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73698,8 +79054,63 @@ int _wrap_BerdyOptions_berdyVariant_get(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilterParameters_ki_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  double arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_ki_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_ki_set" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilterParameters_ki_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->ki = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeMahonyFilterParameters_ki_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  double result;
+  
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_ki_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_ki_get" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  result = (double) ((arg1)->ki);
+  _out = SWIG_From_double(static_cast< double >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
   bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -73707,20 +79118,20 @@ int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(int r
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_use_magnetometer_measurements_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
   ecode2 = SWIG_AsVal_bool(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set" "', argument " "2"" of type '" "bool""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set" "', argument " "2"" of type '" "bool""'");
   } 
   arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->includeAllNetExternalWrenchesAsDynamicVariables = arg2;
+  if (arg1) (arg1)->use_magnetometer_measurements = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73729,22 +79140,22 @@ int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(int r
 }
 
 
-int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_use_magnetometer_measurements_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_use_magnetometer_measurements_get" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (bool) ((arg1)->includeAllNetExternalWrenchesAsDynamicVariables);
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  result = (bool) ((arg1)->use_magnetometer_measurements);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73753,29 +79164,97 @@ int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(int r
 }
 
 
-int _wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  bool arg2 ;
+int _wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  bool val2 ;
+  double val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllJointAccelerationsAsSensors_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->confidence_magnetometer_measurements = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  double result;
+  
+  if (!SWIG_check_num_args("AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  result = (double) ((arg1)->confidence_magnetometer_measurements);
+  _out = SWIG_From_double(static_cast< double >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_AttitudeMahonyFilterParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::AttitudeMahonyFilterParameters *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_AttitudeMahonyFilterParameters",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::AttitudeMahonyFilterParameters *)new iDynTree::AttitudeMahonyFilterParameters();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_AttitudeMahonyFilterParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilterParameters *arg1 = (iDynTree::AttitudeMahonyFilterParameters *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_AttitudeMahonyFilterParameters",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointAccelerationsAsSensors_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeMahonyFilterParameters" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilterParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllJointAccelerationsAsSensors_set" "', argument " "2"" of type '" "bool""'");
-  } 
-  arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->includeAllJointAccelerationsAsSensors = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73784,23 +79263,16 @@ int _wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(int resc, mxArr
 }
 
 
-int _wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_AttitudeMahonyFilter(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  bool result;
+  iDynTree::AttitudeMahonyFilter *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllJointAccelerationsAsSensors_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_AttitudeMahonyFilter",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointAccelerationsAsSensors_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (bool) ((arg1)->includeAllJointAccelerationsAsSensors);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  (void)argv;
+  result = (iDynTree::AttitudeMahonyFilter *)new iDynTree::AttitudeMahonyFilter();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73808,8 +79280,8 @@ int _wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(int resc, mxArr
 }
 
 
-int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_useMagnetoMeterMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
   bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -73817,20 +79289,20 @@ int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(int resc, mxArray *re
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllJointTorquesAsSensors_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_useMagnetoMeterMeasurements",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointTorquesAsSensors_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_useMagnetoMeterMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
   ecode2 = SWIG_AsVal_bool(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllJointTorquesAsSensors_set" "', argument " "2"" of type '" "bool""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilter_useMagnetoMeterMeasurements" "', argument " "2"" of type '" "bool""'");
   } 
   arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->includeAllJointTorquesAsSensors = arg2;
+  (arg1)->useMagnetoMeterMeasurements(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73839,23 +79311,30 @@ int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(int resc, mxArray *re
 }
 
 
-int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllJointTorquesAsSensors_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllJointTorquesAsSensors_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (bool) ((arg1)->includeAllJointTorquesAsSensors);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  (arg1)->setConfidenceForMagnetometerMeasurements(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73863,29 +79342,29 @@ int _wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(int resc, mxArray *re
 }
 
 
-int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  bool arg2 ;
+int _wrap_AttitudeMahonyFilter_setGainkp(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  bool val2 ;
+  double val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsSensors_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setGainkp",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setGainkp" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set" "', argument " "2"" of type '" "bool""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilter_setGainkp" "', argument " "2"" of type '" "double""'");
   } 
-  arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->includeAllNetExternalWrenchesAsSensors = arg2;
+  arg2 = static_cast< double >(val2);
+  (arg1)->setGainkp(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73894,23 +79373,30 @@ int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(int resc, mxAr
 }
 
 
-int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_setGainki(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeAllNetExternalWrenchesAsSensors_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setGainki",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setGainki" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (bool) ((arg1)->includeAllNetExternalWrenchesAsSensors);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilter_setGainki" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  (arg1)->setGainki(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73918,29 +79404,29 @@ int _wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(int resc, mxAr
 }
 
 
-int _wrap_BerdyOptions_includeFixedBaseExternalWrench_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  bool arg2 ;
+int _wrap_AttitudeMahonyFilter_setTimeStepInSeconds(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  bool val2 ;
+  double val2 ;
   int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeFixedBaseExternalWrench_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setTimeStepInSeconds",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeFixedBaseExternalWrench_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setTimeStepInSeconds" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyOptions_includeFixedBaseExternalWrench_set" "', argument " "2"" of type '" "bool""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeMahonyFilter_setTimeStepInSeconds" "', argument " "2"" of type '" "double""'");
   } 
-  arg2 = static_cast< bool >(val2);
-  if (arg1) (arg1)->includeFixedBaseExternalWrench = arg2;
+  arg2 = static_cast< double >(val2);
+  (arg1)->setTimeStepInSeconds(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -73949,23 +79435,33 @@ int _wrap_BerdyOptions_includeFixedBaseExternalWrench_set(int resc, mxArray *res
 }
 
 
-int _wrap_BerdyOptions_includeFixedBaseExternalWrench_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_setGravityDirection(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::Direction *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_includeFixedBaseExternalWrench_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setGravityDirection",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_includeFixedBaseExternalWrench_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setGravityDirection" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (bool) ((arg1)->includeFixedBaseExternalWrench);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_setGravityDirection" "', argument " "2"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_setGravityDirection" "', argument " "2"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Direction * >(argp2);
+  (arg1)->setGravityDirection((iDynTree::Direction const &)*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -73973,30 +79469,34 @@ int _wrap_BerdyOptions_includeFixedBaseExternalWrench_get(int resc, mxArray *res
 }
 
 
-int _wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  std::vector< std::string,std::allocator< std::string > > *arg2 = (std::vector< std::string,std::allocator< std::string > > *) 0 ;
+int _wrap_AttitudeMahonyFilter_setParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::AttitudeMahonyFilterParameters *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setParameters",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setParameters" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_setParameters" "', argument " "2"" of type '" "iDynTree::AttitudeMahonyFilterParameters const &""'"); 
   }
-  arg2 = reinterpret_cast< std::vector< std::string,std::allocator< std::string > > * >(argp2);
-  if (arg1) (arg1)->jointOnWhichTheInternalWrenchIsMeasured = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_setParameters" "', argument " "2"" of type '" "iDynTree::AttitudeMahonyFilterParameters const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp2);
+  result = (bool)(arg1)->setParameters((iDynTree::AttitudeMahonyFilterParameters const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74004,23 +79504,33 @@ int _wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(int resc, mxA
 }
 
 
-int _wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_getParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::AttitudeMahonyFilterParameters *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  std::vector< std::string,std::allocator< std::string > > *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getParameters",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getParameters" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (std::vector< std::string,std::allocator< std::string > > *)& ((arg1)->jointOnWhichTheInternalWrenchIsMeasured);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__AttitudeMahonyFilterParameters,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_getParameters" "', argument " "2"" of type '" "iDynTree::AttitudeMahonyFilterParameters &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_getParameters" "', argument " "2"" of type '" "iDynTree::AttitudeMahonyFilterParameters &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::AttitudeMahonyFilterParameters * >(argp2);
+  (arg1)->getParameters(*arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74028,61 +79538,102 @@ int _wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(int resc, mxA
 }
 
 
-int _wrap_BerdyOptions_baseLink_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_AttitudeMahonyFilter_updateFilterWithMeasurements__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::LinearAccelerometerMeasurements *arg2 = 0 ;
+  iDynTree::GyroscopeMeasurements *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_baseLink_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_updateFilterWithMeasurements",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_baseLink_set" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyOptions_baseLink_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyOptions_baseLink_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
   }
-  if (arg1) (arg1)->baseLink = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinearAccelerometerMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::GyroscopeMeasurements * >(argp3);
+  result = (bool)(arg1)->updateFilterWithMeasurements((iDynTree::LinearAccelerometerMeasurements const &)*arg2,(iDynTree::GyroscopeMeasurements const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_BerdyOptions_baseLink_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_updateFilterWithMeasurements__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::LinearAccelerometerMeasurements *arg2 = 0 ;
+  iDynTree::GyroscopeMeasurements *arg3 = 0 ;
+  iDynTree::MagnetometerMeasurements *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
-  std::string *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_baseLink_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_updateFilterWithMeasurements",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_baseLink_get" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (std::string *) & ((arg1)->baseLink);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinearAccelerometerMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::GyroscopeMeasurements * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "4"" of type '" "iDynTree::MagnetometerMeasurements const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_updateFilterWithMeasurements" "', argument " "4"" of type '" "iDynTree::MagnetometerMeasurements const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::MagnetometerMeasurements * >(argp4);
+  result = (bool)(arg1)->updateFilterWithMeasurements((iDynTree::LinearAccelerometerMeasurements const &)*arg2,(iDynTree::GyroscopeMeasurements const &)*arg3,(iDynTree::MagnetometerMeasurements const &)*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74090,22 +79641,75 @@ int _wrap_BerdyOptions_baseLink_get(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_BerdyOptions_checkConsistency(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_updateFilterWithMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_AttitudeMahonyFilter_updateFilterWithMeasurements__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_AttitudeMahonyFilter_updateFilterWithMeasurements__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'AttitudeMahonyFilter_updateFilterWithMeasurements'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(iDynTree::LinearAccelerometerMeasurements const &,iDynTree::GyroscopeMeasurements const &)\n"
+    "    iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(iDynTree::LinearAccelerometerMeasurements const &,iDynTree::GyroscopeMeasurements const &,iDynTree::MagnetometerMeasurements const &)\n");
+  return 1;
+}
+
+
+int _wrap_AttitudeMahonyFilter_propagateStates(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdyOptions_checkConsistency",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_propagateStates",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyOptions_checkConsistency" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_propagateStates" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  result = (bool)(arg1)->checkConsistency();
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  result = (bool)(arg1)->propagateStates();
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -74114,26 +79718,34 @@ int _wrap_BerdyOptions_checkConsistency(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_delete_BerdyOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyOptions *arg1 = (iDynTree::BerdyOptions *) 0 ;
+int _wrap_AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::Rotation *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_BerdyOptions",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyOptions, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdyOptions" "', argument " "1"" of type '" "iDynTree::BerdyOptions *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyOptions * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Rotation,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix" "', argument " "2"" of type '" "iDynTree::Rotation &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix" "', argument " "2"" of type '" "iDynTree::Rotation &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Rotation * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsRotationMatrix(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74141,30 +79753,34 @@ int _wrap_delete_BerdyOptions(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_BerdySensor_type_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
-  iDynTree::BerdySensorTypes arg2 ;
+int _wrap_AttitudeMahonyFilter_getOrientationEstimateAsQuaternion(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::UnitQuaternion *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_type_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getOrientationEstimateAsQuaternion",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_type_set" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdySensor_type_set" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
-  if (arg1) (arg1)->type = arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion" "', argument " "2"" of type '" "iDynTree::UnitQuaternion &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion" "', argument " "2"" of type '" "iDynTree::UnitQuaternion &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::UnitQuaternion * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsQuaternion(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74172,23 +79788,34 @@ int _wrap_BerdySensor_type_set(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_BerdySensor_type_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+int _wrap_AttitudeMahonyFilter_getOrientationEstimateAsRPY(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::RPY *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::BerdySensorTypes result;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_type_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getOrientationEstimateAsRPY",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_type_get" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsRPY" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  result = (iDynTree::BerdySensorTypes) ((arg1)->type);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsRPY" "', argument " "2"" of type '" "iDynTree::RPY &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_getOrientationEstimateAsRPY" "', argument " "2"" of type '" "iDynTree::RPY &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::RPY * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsRPY(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74196,61 +79823,58 @@ int _wrap_BerdySensor_type_get(int resc, mxArray *resv[], int argc, mxArray *arg
 }
 
 
-int _wrap_BerdySensor_id_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_AttitudeMahonyFilter_getInternalStateSize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
+  size_t result;
   
-  if (!SWIG_check_num_args("BerdySensor_id_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getInternalStateSize",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_id_set" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySensor_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getInternalStateSize" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter const *""'"); 
   }
-  if (arg1) (arg1)->id = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  result = ((iDynTree::AttitudeMahonyFilter const *)arg1)->getInternalStateSize();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_BerdySensor_id_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+int _wrap_AttitudeMahonyFilter_getInternalState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  std::string *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_id_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getInternalState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_id_get" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getInternalState" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  result = (std::string *) & ((arg1)->id);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_getInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_getInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::AttitudeMahonyFilter const *)arg1)->getInternalState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74258,30 +79882,34 @@ int _wrap_BerdySensor_id_get(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_BerdySensor_range_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
-  iDynTree::IndexRange *arg2 = (iDynTree::IndexRange *) 0 ;
+int _wrap_AttitudeMahonyFilter_getDefaultInternalInitialState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_range_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_getDefaultInternalInitialState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_range_set" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_getDefaultInternalInitialState" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_range_set" "', argument " "2"" of type '" "iDynTree::IndexRange *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_getDefaultInternalInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::IndexRange * >(argp2);
-  if (arg1) (arg1)->range = *arg2;
-  _out = (mxArray*)0;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_getDefaultInternalInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::AttitudeMahonyFilter const *)arg1)->getDefaultInternalInitialState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74289,23 +79917,34 @@ int _wrap_BerdySensor_range_set(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_BerdySensor_range_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+int _wrap_AttitudeMahonyFilter_setInternalState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_range_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setInternalState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_range_get" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setInternalState" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  result = (iDynTree::IndexRange *)& ((arg1)->range);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_setInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_setInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->setInternalState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74313,9 +79952,9 @@ int _wrap_BerdySensor_range_get(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
-int _wrap_BerdySensor_eq(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
-  iDynTree::BerdySensor *arg2 = 0 ;
+int _wrap_AttitudeMahonyFilter_setInternalStateInitialOrientation(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -74323,23 +79962,23 @@ int _wrap_BerdySensor_eq(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_eq",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeMahonyFilter_setInternalStateInitialOrientation",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_eq" "', argument " "1"" of type '" "iDynTree::BerdySensor const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeMahonyFilter_setInternalStateInitialOrientation" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdySensor,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_eq" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeMahonyFilter_setInternalStateInitialOrientation" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySensor_eq" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeMahonyFilter_setInternalStateInitialOrientation" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::BerdySensor * >(argp2);
-  result = (bool)((iDynTree::BerdySensor const *)arg1)->operator ==((iDynTree::BerdySensor const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->setInternalStateInitialOrientation((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -74348,34 +79987,26 @@ int _wrap_BerdySensor_eq(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_BerdySensor_lt(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
-  iDynTree::BerdySensor *arg2 = 0 ;
+int _wrap_delete_AttitudeMahonyFilter(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeMahonyFilter *arg1 = (iDynTree::AttitudeMahonyFilter *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdySensor_lt",argc,2,2,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_AttitudeMahonyFilter",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeMahonyFilter, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySensor_lt" "', argument " "1"" of type '" "iDynTree::BerdySensor const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdySensor,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySensor_lt" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeMahonyFilter" "', argument " "1"" of type '" "iDynTree::AttitudeMahonyFilter *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySensor_lt" "', argument " "2"" of type '" "iDynTree::BerdySensor const &""'"); 
+  arg1 = reinterpret_cast< iDynTree::AttitudeMahonyFilter * >(argp1);
+  if (is_owned) {
+    delete arg1;
   }
-  arg2 = reinterpret_cast< iDynTree::BerdySensor * >(argp2);
-  result = (bool)((iDynTree::BerdySensor const *)arg1)->operator <((iDynTree::BerdySensor const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74383,16 +80014,56 @@ int _wrap_BerdySensor_lt(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
 }
 
 
-int _wrap_new_BerdySensor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekf_f(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
-  iDynTree::BerdySensor *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_BerdySensor",argc,0,0,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekf_f",argc,4,4,0)) {
     SWIG_fail;
   }
-  (void)argv;
-  result = (iDynTree::BerdySensor *)new iDynTree::BerdySensor();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdySensor, 1 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_f" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  result = (bool)(arg1)->ekf_f((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74400,26 +80071,45 @@ int _wrap_new_BerdySensor(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_delete_BerdySensor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySensor *arg1 = (iDynTree::BerdySensor *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_BerdySensor",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekf_h",argc,3,3,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySensor, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdySensor" "', argument " "1"" of type '" "iDynTree::BerdySensor *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_h" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySensor * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_h" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_h" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_h" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekf_h" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  result = (bool)(arg1)->ekf_h((iDynTree::VectorDynSize const &)*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74427,30 +80117,45 @@ int _wrap_delete_BerdySensor(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_BerdyDynamicVariable_type_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
-  iDynTree::BerdyDynamicVariablesTypes arg2 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::MatrixDynSize *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_type_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_type_set" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyDynamicVariable_type_set" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
-  if (arg1) (arg1)->type = arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "3"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "3"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp3);
+  result = (bool)(arg1)->ekfComputeJacobianF(*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74458,23 +80163,45 @@ int _wrap_BerdyDynamicVariable_type_set(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_BerdyDynamicVariable_type_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::MatrixDynSize *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
   mxArray * _out;
-  iDynTree::BerdyDynamicVariablesTypes result;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_type_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_type_get" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  result = (iDynTree::BerdyDynamicVariablesTypes) ((arg1)->type);
-  _out = SWIG_From_int(static_cast< int >(result));
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH" "', argument " "3"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH" "', argument " "3"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp3);
+  result = (bool)(arg1)->ekfComputeJacobianH(*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74482,61 +80209,47 @@ int _wrap_BerdyDynamicVariable_type_get(int resc, mxArray *resv[], int argc, mxA
 }
 
 
-int _wrap_BerdyDynamicVariable_id_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
-  std::string *arg2 = 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfPredict(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_id_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfPredict",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_id_set" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyDynamicVariable_id_set" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfPredict" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  if (arg1) (arg1)->id = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  result = (bool)(arg1)->ekfPredict();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_BerdyDynamicVariable_id_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  std::string *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_id_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfUpdate",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_id_get" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfUpdate" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  result = (std::string *) & ((arg1)->id);
-  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  result = (bool)(arg1)->ekfUpdate();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74544,30 +80257,23 @@ int _wrap_BerdyDynamicVariable_id_get(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_BerdyDynamicVariable_range_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
-  iDynTree::IndexRange *arg2 = (iDynTree::IndexRange *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_range_set",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfInit",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_range_set" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_range_set" "', argument " "2"" of type '" "iDynTree::IndexRange *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfInit" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::IndexRange * >(argp2);
-  if (arg1) (arg1)->range = *arg2;
-  _out = (mxArray*)0;
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  result = (bool)(arg1)->ekfInit();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74575,23 +80281,22 @@ int _wrap_BerdyDynamicVariable_range_set(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_BerdyDynamicVariable_range_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_range_get",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfReset",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_range_get" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  result = (iDynTree::IndexRange *)& ((arg1)->range);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IndexRange, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  (arg1)->ekfReset();
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74599,9 +80304,9 @@ int _wrap_BerdyDynamicVariable_range_get(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_BerdyDynamicVariable_eq(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
-  iDynTree::BerdyDynamicVariable *arg2 = 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -74609,23 +80314,23 @@ int _wrap_BerdyDynamicVariable_eq(int resc, mxArray *resv[], int argc, mxArray *
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_eq",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_eq" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdyDynamicVariable,  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_eq" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyDynamicVariable_eq" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp2);
-  result = (bool)((iDynTree::BerdyDynamicVariable const *)arg1)->operator ==((iDynTree::BerdyDynamicVariable const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->ekfSetMeasurementVector((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -74634,9 +80339,9 @@ int _wrap_BerdyDynamicVariable_eq(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_BerdyDynamicVariable_lt(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
-  iDynTree::BerdyDynamicVariable *arg2 = 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
@@ -74644,23 +80349,23 @@ int _wrap_BerdyDynamicVariable_lt(int resc, mxArray *resv[], int argc, mxArray *
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdyDynamicVariable_lt",argc,2,2,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetInputVector",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyDynamicVariable_lt" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__BerdyDynamicVariable,  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyDynamicVariable_lt" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyDynamicVariable_lt" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariable const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp2);
-  result = (bool)((iDynTree::BerdyDynamicVariable const *)arg1)->operator <((iDynTree::BerdyDynamicVariable const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->ekfSetInputVector((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -74669,60 +80374,34 @@ int _wrap_BerdyDynamicVariable_lt(int resc, mxArray *resv[], int argc, mxArray *
 }
 
 
-int _wrap_new_BerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::BerdyDynamicVariable *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_BerdyDynamicVariable",argc,0,0,0)) {
-    SWIG_fail;
-  }
-  (void)argv;
-  result = (iDynTree::BerdyDynamicVariable *)new iDynTree::BerdyDynamicVariable();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdyDynamicVariable, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_delete_BerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyDynamicVariable *arg1 = (iDynTree::BerdyDynamicVariable *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInitialState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_BerdyDynamicVariable",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetInitialState",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyDynamicVariable, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdyDynamicVariable" "', argument " "1"" of type '" "iDynTree::BerdyDynamicVariable *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyDynamicVariable * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_BerdyHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::BerdyHelper *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_BerdyHelper",argc,0,0,0)) {
-    SWIG_fail;
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  (void)argv;
-  result = (iDynTree::BerdyHelper *)new iDynTree::BerdyHelper();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdyHelper, 1 |  0 );
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->ekfSetInitialState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74730,23 +80409,34 @@ int _wrap_new_BerdyHelper(int resc, mxArray *resv[], int argc, mxArray *argv[])
 }
 
 
-int _wrap_BerdyHelper_model__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_model",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_model" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (iDynTree::Model *) &(arg1)->model();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->ekfSetStateCovariance((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74754,23 +80444,34 @@ int _wrap_BerdyHelper_model__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_BerdyHelper_sensors__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::SensorsList *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_sensors",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_sensors" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (iDynTree::SensorsList *) &(arg1)->sensors();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->ekfSetSystemNoiseCovariance((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74778,23 +80479,34 @@ int _wrap_BerdyHelper_sensors__SWIG_0(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_BerdyHelper_dynamicTraversal(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::Traversal *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_dynamicTraversal",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_dynamicTraversal" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (iDynTree::Traversal *) &((iDynTree::BerdyHelper const *)arg1)->dynamicTraversal();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Traversal, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->ekfSetMeasurementNoiseCovariance((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74802,23 +80514,30 @@ int _wrap_BerdyHelper_dynamicTraversal(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_BerdyHelper_model__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateSize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::Model *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_model",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetStateSize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_model" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (iDynTree::Model *) &((iDynTree::BerdyHelper const *)arg1)->model();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->ekfSetStateSize(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74826,51 +80545,30 @@ int _wrap_BerdyHelper_model__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_BerdyHelper_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdyHelper_model__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdyHelper_model__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_model'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdyHelper::model()\n"
-    "    iDynTree::BerdyHelper::model() const\n");
-  return 1;
-}
-
-
-int _wrap_BerdyHelper_sensors__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputSize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SensorsList *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_sensors",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetInputSize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_sensors" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (iDynTree::SensorsList *) &((iDynTree::BerdyHelper const *)arg1)->sensors();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->ekfSetInputSize(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74878,51 +80576,30 @@ int _wrap_BerdyHelper_sensors__SWIG_1(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_BerdyHelper_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdyHelper_sensors__SWIG_0(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdyHelper_sensors__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_sensors'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdyHelper::sensors()\n"
-    "    iDynTree::BerdyHelper::sensors() const\n");
-  return 1;
-}
-
-
-int _wrap_BerdyHelper_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  size_t arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_isValid",argc,1,1,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_isValid" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (bool)((iDynTree::BerdyHelper const *)arg1)->isValid();
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  (arg1)->ekfSetOutputSize(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -74930,58 +80607,33 @@ int _wrap_BerdyHelper_isValid(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
-int _wrap_BerdyHelper_init__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::SensorsList *arg3 = 0 ;
-  iDynTree::BerdyOptions arg4 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStates(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_init",argc,4,4,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfGetStates",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_init" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfGetStates" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfGetStates" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
-  {
-    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__BerdyOptions,  0 );
-    if (!SWIG_IsOK(res4)) {
-      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_init" "', argument " "4"" of type '" "iDynTree::BerdyOptions const""'"); 
-    }  
-    if (!argp4) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "4"" of type '" "iDynTree::BerdyOptions const""'");
-    } else {
-      arg4 = *(reinterpret_cast< iDynTree::BerdyOptions * >(argp4));
-    }
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfGetStates" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3,arg4);
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::DiscreteExtendedKalmanFilterHelper const *)arg1)->ekfGetStates((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -74990,44 +80642,33 @@ int _wrap_BerdyHelper_init__SWIG_0(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_BerdyHelper_init__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::Model *arg2 = 0 ;
-  iDynTree::SensorsList *arg3 = 0 ;
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_init",argc,3,3,0)) {
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_init" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
-  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3);
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::DiscreteExtendedKalmanFilterHelper const *)arg1)->ekfGetStateCovariance((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -75036,76 +80677,26 @@ int _wrap_BerdyHelper_init__SWIG_1(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_BerdyHelper_init(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 3) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          return _wrap_BerdyHelper_init__SWIG_1(resc,resv,argc,argv);
-        }
-      }
-    }
-  }
-  if (argc == 4) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__BerdyOptions, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            return _wrap_BerdyHelper_init__SWIG_0(resc,resv,argc,argv);
-          }
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_init'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdyHelper::init(iDynTree::Model const &,iDynTree::SensorsList const &,iDynTree::BerdyOptions const)\n"
-    "    iDynTree::BerdyHelper::init(iDynTree::Model const &,iDynTree::SensorsList const &)\n");
-  return 1;
-}
-
-
-int _wrap_BerdyHelper_getOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_delete_DiscreteExtendedKalmanFilterHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::BerdyOptions result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getOptions",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_DiscreteExtendedKalmanFilterHelper",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getOptions" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_DiscreteExtendedKalmanFilterHelper" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getOptions();
-  _out = SWIG_NewPointerObj((new iDynTree::BerdyOptions(static_cast< const iDynTree::BerdyOptions& >(result))), SWIGTYPE_p_iDynTree__BerdyOptions, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75113,47 +80704,48 @@ int _wrap_BerdyHelper_getOptions(int resc, mxArray *resv[], int argc, mxArray *a
 }
 
 
-int _wrap_BerdyHelper_getNrOfDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  size_t result;
-  
-  if (!SWIG_check_num_args("BerdyHelper_getNrOfDynamicVariables",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNrOfDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getNrOfDynamicVariables();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
-  if (_out) --resc, *resv++ = _out;
+SWIGINTERN int _wrap_output_dimensions_with_magnetometer_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(iDynTree::output_dimensions_with_magnetometer));
   return 0;
-fail:
-  return 1;
 }
 
 
-int _wrap_BerdyHelper_getNrOfDynamicEquations(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+SWIGINTERN int _wrap_output_dimensions_without_magnetometer_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(iDynTree::output_dimensions_without_magnetometer));
+  return 0;
+}
+
+
+SWIGINTERN int _wrap_input_dimensions_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  resv[0] = SWIG_From_unsigned_SS_int(static_cast< unsigned int >(iDynTree::input_dimensions));
+  return 0;
+}
+
+
+int _wrap_AttitudeEstimatorState_m_orientation_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  iDynTree::UnitQuaternion *arg2 = (iDynTree::UnitQuaternion *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  size_t result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getNrOfDynamicEquations",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_orientation_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNrOfDynamicEquations" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_orientation_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getNrOfDynamicEquations();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_orientation_set" "', argument " "2"" of type '" "iDynTree::UnitQuaternion *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::UnitQuaternion * >(argp2);
+  if (arg1) (arg1)->m_orientation = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75161,23 +80753,23 @@ int _wrap_BerdyHelper_getNrOfDynamicEquations(int resc, mxArray *resv[], int arg
 }
 
 
-int _wrap_BerdyHelper_getNrOfSensorsMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_AttitudeEstimatorState_m_orientation_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  size_t result;
+  iDynTree::UnitQuaternion *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_getNrOfSensorsMeasurements",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_orientation_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNrOfSensorsMeasurements" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_orientation_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getNrOfSensorsMeasurements();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  result = (iDynTree::UnitQuaternion *)& ((arg1)->m_orientation);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75185,67 +80777,30 @@ int _wrap_BerdyHelper_getNrOfSensorsMeasurements(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg4 = 0 ;
-  iDynTree::VectorDynSize *arg5 = 0 ;
+int _wrap_AttitudeEstimatorState_m_angular_velocity_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_resizeAndZeroBerdyMatrices",argc,5,5,0)) {
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_angular_velocity_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_angular_velocity_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_angular_velocity_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
   }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
-  result = (bool)(arg1)->resizeAndZeroBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
+  if (arg1) (arg1)->m_angular_velocity = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75253,67 +80808,23 @@ int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_0(int resc, mxArray *resv
 }
 
 
-int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::MatrixDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::MatrixDynSize *arg4 = 0 ;
-  iDynTree::VectorDynSize *arg5 = 0 ;
+int _wrap_AttitudeEstimatorState_m_angular_velocity_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::Vector3 *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_resizeAndZeroBerdyMatrices",argc,5,5,0)) {
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_angular_velocity_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_angular_velocity_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_resizeAndZeroBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
-  result = (bool)(arg1)->resizeAndZeroBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  result = (iDynTree::Vector3 *)& ((arg1)->m_angular_velocity);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75321,135 +80832,30 @@ int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_1(int resc, mxArray *resv
 }
 
 
-int _wrap_BerdyHelper_resizeAndZeroBerdyMatrices(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 5) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            void *vptr = 0;
-            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-            _v = SWIG_CheckState(res);
-            if (_v) {
-              return _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_0(resc,resv,argc,argv);
-            }
-          }
-        }
-      }
-    }
-  }
-  if (argc == 5) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            void *vptr = 0;
-            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-            _v = SWIG_CheckState(res);
-            if (_v) {
-              return _wrap_BerdyHelper_resizeAndZeroBerdyMatrices__SWIG_1(resc,resv,argc,argv);
-            }
-          }
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_resizeAndZeroBerdyMatrices'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdyHelper::resizeAndZeroBerdyMatrices(iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &,iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &)\n"
-    "    iDynTree::BerdyHelper::resizeAndZeroBerdyMatrices(iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &,iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &)\n");
-  return 1;
-}
-
-
-int _wrap_BerdyHelper_getBerdyMatrices__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg4 = 0 ;
-  iDynTree::VectorDynSize *arg5 = 0 ;
+int _wrap_AttitudeEstimatorState_m_gyroscope_bias_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 = 0 ;
   int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getBerdyMatrices",argc,5,5,0)) {
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_gyroscope_bias_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
   }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
-  result = (bool)(arg1)->getBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
+  if (arg1) (arg1)->m_gyroscope_bias = *arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75457,67 +80863,23 @@ int _wrap_BerdyHelper_getBerdyMatrices__SWIG_0(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_BerdyHelper_getBerdyMatrices__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::MatrixDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::MatrixDynSize *arg4 = 0 ;
-  iDynTree::VectorDynSize *arg5 = 0 ;
+int _wrap_AttitudeEstimatorState_m_gyroscope_bias_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
+  iDynTree::Vector3 *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_getBerdyMatrices",argc,5,5,0)) {
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_gyroscope_bias_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "2"" of type '" "iDynTree::MatrixDynSize &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getBerdyMatrices" "', argument " "5"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::VectorDynSize * >(argp5);
-  result = (bool)(arg1)->getBerdyMatrices(*arg2,*arg3,*arg4,*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  result = (iDynTree::Vector3 *)& ((arg1)->m_gyroscope_bias);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75525,91 +80887,16 @@ int _wrap_BerdyHelper_getBerdyMatrices__SWIG_1(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_BerdyHelper_getBerdyMatrices(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 5) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            void *vptr = 0;
-            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-            _v = SWIG_CheckState(res);
-            if (_v) {
-              return _wrap_BerdyHelper_getBerdyMatrices__SWIG_0(resc,resv,argc,argv);
-            }
-          }
-        }
-      }
-    }
-  }
-  if (argc == 5) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdyHelper, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        void *vptr = 0;
-        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-        _v = SWIG_CheckState(res);
-        if (_v) {
-          void *vptr = 0;
-          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
-          _v = SWIG_CheckState(res);
-          if (_v) {
-            void *vptr = 0;
-            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-            _v = SWIG_CheckState(res);
-            if (_v) {
-              return _wrap_BerdyHelper_getBerdyMatrices__SWIG_1(resc,resv,argc,argv);
-            }
-          }
-        }
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdyHelper_getBerdyMatrices'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdyHelper::getBerdyMatrices(iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &,iDynTree::SparseMatrix< iDynTree::ColumnMajor > &,iDynTree::VectorDynSize &)\n"
-    "    iDynTree::BerdyHelper::getBerdyMatrices(iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &,iDynTree::MatrixDynSize &,iDynTree::VectorDynSize &)\n");
-  return 1;
-}
-
-
-int _wrap_BerdyHelper_getSensorsOrdering(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
+int _wrap_new_AttitudeEstimatorState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
-  std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > *result = 0 ;
+  iDynTree::AttitudeEstimatorState *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_getSensorsOrdering",argc,1,1,0)) {
+  if (!SWIG_check_num_args("new_AttitudeEstimatorState",argc,0,0,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getSensorsOrdering" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > *) &((iDynTree::BerdyHelper const *)arg1)->getSensorsOrdering();
-  _out = swig::from(static_cast< std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > >(*result));
+  (void)argv;
+  result = (iDynTree::AttitudeEstimatorState *)new iDynTree::AttitudeEstimatorState();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75617,39 +80904,26 @@ int _wrap_BerdyHelper_getSensorsOrdering(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_BerdyHelper_getRangeSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::SensorType arg2 ;
-  unsigned int arg3 ;
+int _wrap_delete_AttitudeEstimatorState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  unsigned int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeSensorVariable",argc,3,3,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_AttitudeEstimatorState",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeEstimatorState" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeSensorVariable" "', argument " "2"" of type '" "iDynTree::SensorType""'");
-  } 
-  arg2 = static_cast< iDynTree::SensorType >(val2);
-  ecode3 = SWIG_AsVal_unsigned_SS_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeSensorVariable" "', argument " "3"" of type '" "unsigned int""'");
-  } 
-  arg3 = static_cast< unsigned int >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeSensorVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75657,39 +80931,30 @@ int _wrap_BerdyHelper_getRangeSensorVariable(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_BerdyHelper_getRangeDOFSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::BerdySensorTypes arg2 ;
-  iDynTree::DOFIndex arg3 ;
+int _wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  double val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeDOFSensorVariable",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_time_step_in_seconds_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeDOFSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_time_step_in_seconds_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeDOFSensorVariable" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeDOFSensorVariable" "', argument " "3"" of type '" "iDynTree::DOFIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_time_step_in_seconds_set" "', argument " "2"" of type '" "double""'");
   } 
-  arg3 = static_cast< iDynTree::DOFIndex >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeDOFSensorVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->time_step_in_seconds = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75697,39 +80962,23 @@ int _wrap_BerdyHelper_getRangeDOFSensorVariable(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_BerdyHelper_getRangeJointSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::BerdySensorTypes arg2 ;
-  iDynTree::JointIndex arg3 ;
+int _wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeJointSensorVariable",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_time_step_in_seconds_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeJointSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_time_step_in_seconds_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeJointSensorVariable" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeJointSensorVariable" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::JointIndex >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeJointSensorVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->time_step_in_seconds);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75737,39 +80986,30 @@ int _wrap_BerdyHelper_getRangeJointSensorVariable(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_BerdyHelper_getRangeLinkSensorVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::BerdySensorTypes arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  double val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeLinkSensorVariable",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeLinkSensorVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeLinkSensorVariable" "', argument " "2"" of type '" "iDynTree::BerdySensorTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdySensorTypes >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeLinkSensorVariable" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeLinkSensorVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->bias_correlation_time_factor = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75777,39 +81017,23 @@ int _wrap_BerdyHelper_getRangeLinkSensorVariable(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_BerdyHelper_getRangeLinkVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::BerdyDynamicVariablesTypes arg2 ;
-  iDynTree::LinkIndex arg3 ;
+int _wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeLinkVariable",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeLinkVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeLinkVariable" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeLinkVariable" "', argument " "3"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::LinkIndex >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeLinkVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->bias_correlation_time_factor);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75817,39 +81041,30 @@ int _wrap_BerdyHelper_getRangeLinkVariable(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_BerdyHelper_getRangeJointVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::BerdyDynamicVariablesTypes arg2 ;
-  iDynTree::JointIndex arg3 ;
+int _wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  double val2 ;
   int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeJointVariable",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeJointVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeJointVariable" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeJointVariable" "', argument " "3"" of type '" "iDynTree::JointIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set" "', argument " "2"" of type '" "double""'");
   } 
-  arg3 = static_cast< iDynTree::JointIndex >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeJointVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->accelerometer_noise_variance = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75857,39 +81072,23 @@ int _wrap_BerdyHelper_getRangeJointVariable(int resc, mxArray *resv[], int argc,
 }
 
 
-int _wrap_BerdyHelper_getRangeDOFVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::BerdyDynamicVariablesTypes arg2 ;
-  iDynTree::DOFIndex arg3 ;
+int _wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  int val3 ;
-  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::IndexRange result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getRangeDOFVariable",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getRangeDOFVariable" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getRangeDOFVariable" "', argument " "2"" of type '" "iDynTree::BerdyDynamicVariablesTypes""'");
-  } 
-  arg2 = static_cast< iDynTree::BerdyDynamicVariablesTypes >(val2);
-  ecode3 = SWIG_AsVal_int(argv[2], &val3);
-  if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "BerdyHelper_getRangeDOFVariable" "', argument " "3"" of type '" "iDynTree::DOFIndex""'");
-  } 
-  arg3 = static_cast< iDynTree::DOFIndex >(val3);
-  result = ((iDynTree::BerdyHelper const *)arg1)->getRangeDOFVariable(arg2,arg3);
-  _out = SWIG_NewPointerObj((new iDynTree::IndexRange(static_cast< const iDynTree::IndexRange& >(result))), SWIGTYPE_p_iDynTree__IndexRange, SWIG_POINTER_OWN |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->accelerometer_noise_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75897,23 +81096,30 @@ int _wrap_BerdyHelper_getRangeDOFVariable(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_BerdyHelper_getDynamicVariablesOrdering(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdyHelper_getDynamicVariablesOrdering",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getDynamicVariablesOrdering" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > *) &((iDynTree::BerdyHelper const *)arg1)->getDynamicVariablesOrdering();
-  _out = swig::from(static_cast< std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > >(*result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->magnetometer_noise_variance = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -75921,100 +81127,23 @@ int _wrap_BerdyHelper_getDynamicVariablesOrdering(int resc, mxArray *resv[], int
 }
 
 
-int _wrap_BerdyHelper_serializeDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::LinkProperAccArray *arg2 = 0 ;
-  iDynTree::LinkNetTotalWrenchesWithoutGravity *arg3 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg4 = 0 ;
-  iDynTree::LinkInternalWrenches *arg5 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg6 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg7 = 0 ;
-  iDynTree::VectorDynSize *arg8 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
-  void *argp8 = 0 ;
-  int res8 = 0 ;
   mxArray * _out;
-  bool result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_serializeDynamicVariables",argc,8,8,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkAccArray,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "2"" of type '" "iDynTree::LinkProperAccArray &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "2"" of type '" "iDynTree::LinkProperAccArray &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::LinkProperAccArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetTotalWrenchesWithoutGravity &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::LinkNetTotalWrenchesWithoutGravity * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "4"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "4"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "5"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "5"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "6"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "6"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "7"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp7);
-  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res8)) {
-    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "8"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp8) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariables" "', argument " "8"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg8 = reinterpret_cast< iDynTree::VectorDynSize * >(argp8);
-  result = (bool)(arg1)->serializeDynamicVariables(*arg2,*arg3,*arg4,*arg5,*arg6,*arg7,*arg8);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->magnetometer_noise_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76022,89 +81151,30 @@ int _wrap_BerdyHelper_serializeDynamicVariables(int resc, mxArray *resv[], int a
 }
 
 
-int _wrap_BerdyHelper_serializeSensorVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::SensorsMeasurements *arg2 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg4 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg5 = 0 ;
-  iDynTree::LinkInternalWrenches *arg6 = 0 ;
-  iDynTree::VectorDynSize *arg7 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
-  void *argp5 = 0 ;
-  int res5 = 0 ;
-  void *argp6 = 0 ;
-  int res6 = 0 ;
-  void *argp7 = 0 ;
-  int res7 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_serializeSensorVariables",argc,7,7,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SensorsMeasurements,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "2"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "2"" of type '" "iDynTree::SensorsMeasurements &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::SensorsMeasurements * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "4"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "5"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "6"" of type '" "iDynTree::LinkInternalWrenches &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::LinkInternalWrenches * >(argp6);
-  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res7)) {
-    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "7"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp7) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeSensorVariables" "', argument " "7"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg7 = reinterpret_cast< iDynTree::VectorDynSize * >(argp7);
-  result = (bool)(arg1)->serializeSensorVariables(*arg2,*arg3,*arg4,*arg5,*arg6,*arg7);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->gyroscope_noise_variance = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76112,56 +81182,23 @@ int _wrap_BerdyHelper_serializeSensorVariables(int resc, mxArray *resv[], int ar
 }
 
 
-int _wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::JointDOFsDoubleArray *arg2 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
   mxArray * _out;
-  bool result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA",argc,4,4,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "2"" of type '" "iDynTree::JointDOFsDoubleArray &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  result = (bool)(arg1)->serializeDynamicVariablesComputedFromFixedBaseRNEA(*arg2,*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->gyroscope_noise_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76169,56 +81206,30 @@ int _wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(int res
 }
 
 
-int _wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::VectorDynSize *arg3 = 0 ;
-  iDynTree::VectorDynSize *arg4 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 = 0 ;
-  int res4 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_extractJointTorquesFromDynamicVariables",argc,4,4,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractJointTorquesFromDynamicVariables" "', argument " "4"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
-  result = (bool)((iDynTree::BerdyHelper const *)arg1)->extractJointTorquesFromDynamicVariables((iDynTree::VectorDynSize const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->gyro_bias_noise_variance = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76226,45 +81237,23 @@ int _wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(int resc, mxArray
 }
 
 
-int _wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  iDynTree::LinkNetExternalWrenches *arg3 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
   mxArray * _out;
-  bool result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables" "', argument " "3"" of type '" "iDynTree::LinkNetExternalWrenches &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp3);
-  result = (bool)((iDynTree::BerdyHelper const *)arg1)->extractLinkNetExternalWrenchesFromDynamicVariables((iDynTree::VectorDynSize const &)*arg2,*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->gyro_bias_noise_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76272,66 +81261,30 @@ int _wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(int res
 }
 
 
-int _wrap_BerdyHelper_updateKinematicsFromFloatingBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::FrameIndex *arg4 = 0 ;
-  iDynTree::Vector3 *arg5 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  iDynTree::FrameIndex temp4 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_updateKinematicsFromFloatingBase",argc,5,5,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set" "', argument " "2"" of type '" "double""'");
   } 
-  temp4 = static_cast< iDynTree::FrameIndex >(val4);
-  arg4 = &temp4;
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
-  result = (bool)(arg1)->updateKinematicsFromFloatingBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::FrameIndex const &)*arg4,(iDynTree::Vector3 const &)*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->initial_orientation_error_variance = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76339,66 +81292,23 @@ int _wrap_BerdyHelper_updateKinematicsFromFloatingBase(int resc, mxArray *resv[]
 }
 
 
-int _wrap_BerdyHelper_updateKinematicsFromFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::FrameIndex *arg4 = 0 ;
-  iDynTree::Vector3 *arg5 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  iDynTree::FrameIndex temp4 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
   mxArray * _out;
-  bool result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_updateKinematicsFromFixedBase",argc,5,5,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  temp4 = static_cast< iDynTree::FrameIndex >(val4);
-  arg4 = &temp4;
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
-  result = (bool)(arg1)->updateKinematicsFromFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::FrameIndex const &)*arg4,(iDynTree::Vector3 const &)*arg5);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->initial_orientation_error_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76406,56 +81316,30 @@ int _wrap_BerdyHelper_updateKinematicsFromFixedBase(int resc, mxArray *resv[], i
 }
 
 
-int _wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::Vector3 *arg4 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  void *argp4 ;
-  int res4 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_updateKinematicsFromTraversalFixedBase",argc,4,4,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res4)) {
-    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp4) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_updateKinematicsFromTraversalFixedBase" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg4 = reinterpret_cast< iDynTree::Vector3 * >(argp4);
-  result = (bool)(arg1)->updateKinematicsFromTraversalFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,(iDynTree::Vector3 const &)*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->initial_ang_vel_error_variance = arg2;
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76463,42 +81347,23 @@ int _wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(int resc, mxArray *
 }
 
 
-int _wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::Transform *arg3 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
-  int ecode2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
   mxArray * _out;
-  bool result;
+  double result;
   
-  if (!SWIG_check_num_args("BerdyHelper_setNetExternalWrenchMeasurementFrame",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
-  if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
-  } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_setNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
-  result = (bool)(arg1)->setNetExternalWrenchMeasurementFrame(arg2,(iDynTree::Transform const &)*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->initial_ang_vel_error_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76506,42 +81371,54 @@ int _wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(int resc, mxArray *re
 }
 
 
-int _wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
-  iDynTree::LinkIndex arg2 ;
-  iDynTree::Transform *arg3 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  double val2 ;
   int ecode2 = 0 ;
-  void *argp3 = 0 ;
-  int res3 = 0 ;
   mxArray * _out;
-  bool result;
   
-  if (!SWIG_check_num_args("BerdyHelper_getNetExternalWrenchMeasurementFrame",argc,3,3,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "1"" of type '" "iDynTree::BerdyHelper const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set" "', argument " "2"" of type '" "double""'");
   } 
-  arg2 = static_cast< iDynTree::LinkIndex >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform &""'"); 
+  arg2 = static_cast< double >(val2);
+  if (arg1) (arg1)->initial_gyro_bias_error_variance = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  double result;
+  
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdyHelper_getNetExternalWrenchMeasurementFrame" "', argument " "3"" of type '" "iDynTree::Transform &""'"); 
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
-  result = (bool)((iDynTree::BerdyHelper const *)arg1)->getNetExternalWrenchMeasurementFrame(arg2,*arg3);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (double) ((arg1)->initial_gyro_bias_error_variance);
+  _out = SWIG_From_double(static_cast< double >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76549,25 +81426,29 @@ int _wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(int resc, mxArray *re
 }
 
 
-int _wrap_delete_BerdyHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = (iDynTree::BerdyHelper *) 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_BerdyHelper",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set",argc,2,2,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdyHelper, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdyHelper" "', argument " "1"" of type '" "iDynTree::BerdyHelper *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  if (is_owned) {
-    delete arg1;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->use_magnetometer_measurements = arg2;
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -76576,26 +81457,40 @@ int _wrap_delete_BerdyHelper(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
-int _wrap_new_BerdySparseMAPSolver(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdyHelper *arg1 = 0 ;
+int _wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::BerdySparseMAPSolver *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("new_BerdySparseMAPSolver",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__BerdyHelper,  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "new_BerdySparseMAPSolver" "', argument " "1"" of type '" "iDynTree::BerdyHelper &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  if (!argp1) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "new_BerdySparseMAPSolver" "', argument " "1"" of type '" "iDynTree::BerdyHelper &""'"); 
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
+  result = (bool) ((arg1)->use_magnetometer_measurements);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_AttitudeQuaternionEKFParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::AttitudeQuaternionEKFParameters *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_AttitudeQuaternionEKFParameters",argc,0,0,0)) {
+    SWIG_fail;
   }
-  arg1 = reinterpret_cast< iDynTree::BerdyHelper * >(argp1);
-  result = (iDynTree::BerdySparseMAPSolver *)new iDynTree::BerdySparseMAPSolver(*arg1);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 1 |  0 );
+  (void)argv;
+  result = (iDynTree::AttitudeQuaternionEKFParameters *)new iDynTree::AttitudeQuaternionEKFParameters();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, 1 |  0 );
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76603,22 +81498,22 @@ int _wrap_new_BerdySparseMAPSolver(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_delete_BerdySparseMAPSolver(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_delete_AttitudeQuaternionEKFParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
   
   int is_owned;
-  if (!SWIG_check_num_args("delete_BerdySparseMAPSolver",argc,1,1,0)) {
+  if (!SWIG_check_num_args("delete_AttitudeQuaternionEKFParameters",argc,1,1,0)) {
     SWIG_fail;
   }
   is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_BerdySparseMAPSolver" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeQuaternionEKFParameters" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKFParameters *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp1);
   if (is_owned) {
     delete arg1;
   }
@@ -76630,32 +81525,49 @@ int _wrap_delete_BerdySparseMAPSolver(int resc, mxArray *resv[], int argc, mxArr
 }
 
 
-int _wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
+int _wrap_new_AttitudeQuaternionEKF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::AttitudeQuaternionEKF *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_AttitudeQuaternionEKF",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::AttitudeQuaternionEKF *)new iDynTree::AttitudeQuaternionEKF();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeQuaternionEKF_getParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::AttitudeQuaternionEKFParameters *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
+  void *argp2 = 0 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getParameters",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getParameters" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_getParameters" "', argument " "2"" of type '" "iDynTree::AttitudeQuaternionEKFParameters &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_getParameters" "', argument " "2"" of type '" "iDynTree::AttitudeQuaternionEKFParameters &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
-  (arg1)->setDynamicsConstraintsPriorCovariance((iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp2);
+  (arg1)->getParameters(*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -76664,32 +81576,32 @@ int _wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(int resc, m
 }
 
 
-int _wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
+int _wrap_AttitudeQuaternionEKF_setParameters(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::AttitudeQuaternionEKFParameters *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setParameters",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setParameters" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__AttitudeQuaternionEKFParameters,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_setParameters" "', argument " "2"" of type '" "iDynTree::AttitudeQuaternionEKFParameters const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_setParameters" "', argument " "2"" of type '" "iDynTree::AttitudeQuaternionEKFParameters const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
-  (arg1)->setDynamicsRegularizationPriorCovariance((iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::AttitudeQuaternionEKFParameters * >(argp2);
+  (arg1)->setParameters((iDynTree::AttitudeQuaternionEKFParameters const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -76698,32 +81610,32 @@ int _wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(int resc
 }
 
 
-int _wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
+int _wrap_AttitudeQuaternionEKF_setGravityDirection(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::Direction *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setGravityDirection",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setGravityDirection" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Direction,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_setGravityDirection" "', argument " "2"" of type '" "iDynTree::Direction const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_setGravityDirection" "', argument " "2"" of type '" "iDynTree::Direction const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  (arg1)->setDynamicsRegularizationPriorExpectedValue((iDynTree::VectorDynSize const &)*arg2);
+  arg2 = reinterpret_cast< iDynTree::Direction * >(argp2);
+  (arg1)->setGravityDirection((iDynTree::Direction const &)*arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -76732,32 +81644,29 @@ int _wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(int r
 }
 
 
-int _wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *arg2 = 0 ;
+int _wrap_AttitudeQuaternionEKF_setTimeStepInSeconds(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_setMeasurementsPriorCovariance",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setTimeStepInSeconds",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_setMeasurementsPriorCovariance" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_setMeasurementsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_setMeasurementsPriorCovariance" "', argument " "2"" of type '" "iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setTimeStepInSeconds" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::SparseMatrix< iDynTree::ColumnMajor > * >(argp2);
-  (arg1)->setMeasurementsPriorCovariance((iDynTree::SparseMatrix< iDynTree::ColumnMajor > const &)*arg2);
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKF_setTimeStepInSeconds" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  (arg1)->setTimeStepInSeconds(arg2);
   _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -76766,23 +81675,30 @@ int _wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(int resc, mxArray
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_setBiasCorrelationTimeFactor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  double arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setBiasCorrelationTimeFactor",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->dynamicsConstraintsPriorCovarianceInverse();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  (arg1)->setBiasCorrelationTimeFactor(arg2);
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76790,23 +81706,31 @@ int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_0
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_useMagnetometerMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  bool arg2 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
   mxArray * _out;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_useMagnetometerMeasurements",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_useMagnetometerMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &(arg1)->dynamicsConstraintsPriorCovarianceInverse();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKF_useMagnetometerMeasurements" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  result = (bool)(arg1)->useMagnetometerMeasurements(arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76814,51 +81738,79 @@ int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_1
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_1(resc,resv,argc,argv);
-    }
+int _wrap_AttitudeQuaternionEKF_setMeasurementNoiseVariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  double arg2 ;
+  double arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setMeasurementNoiseVariance",argc,3,3,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse__SWIG_0(resc,resv,argc,argv);
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setMeasurementNoiseVariance" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdySparseMAPSolver::dynamicsConstraintsPriorCovarianceInverse() const\n"
-    "    iDynTree::BerdySparseMAPSolver::dynamicsConstraintsPriorCovarianceInverse()\n");
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKF_setMeasurementNoiseVariance" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "AttitudeQuaternionEKF_setMeasurementNoiseVariance" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  result = (bool)(arg1)->setMeasurementNoiseVariance(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_setSystemNoiseVariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  double arg2 ;
+  double arg3 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
   mxArray * _out;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setSystemNoiseVariance",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setSystemNoiseVariance" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->dynamicsRegularizationPriorCovarianceInverse();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKF_setSystemNoiseVariance" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "AttitudeQuaternionEKF_setSystemNoiseVariance" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  result = (bool)(arg1)->setSystemNoiseVariance(arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76866,23 +81818,47 @@ int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWI
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_setInitialStateCovariance(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  double arg2 ;
+  double arg3 ;
+  double arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
+  double val4 ;
+  int ecode4 = 0 ;
   mxArray * _out;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setInitialStateCovariance",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setInitialStateCovariance" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &(arg1)->dynamicsRegularizationPriorCovarianceInverse();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "AttitudeQuaternionEKF_setInitialStateCovariance" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "AttitudeQuaternionEKF_setInitialStateCovariance" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  ecode4 = SWIG_AsVal_double(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "AttitudeQuaternionEKF_setInitialStateCovariance" "', argument " "4"" of type '" "double""'");
+  } 
+  arg4 = static_cast< double >(val4);
+  result = (bool)(arg1)->setInitialStateCovariance(arg2,arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76890,51 +81866,69 @@ int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWI
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_1(resc,resv,argc,argv);
-    }
+int _wrap_AttitudeQuaternionEKF_initializeFilter(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_initializeFilter",argc,1,1,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse__SWIG_0(resc,resv,argc,argv);
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_initializeFilter" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorCovarianceInverse() const\n"
-    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorCovarianceInverse()\n");
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  result = (bool)(arg1)->initializeFilter();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::LinearAccelerometerMeasurements *arg2 = 0 ;
+  iDynTree::GyroscopeMeasurements *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
   mxArray * _out;
-  iDynTree::VectorDynSize *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_updateFilterWithMeasurements",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::VectorDynSize *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->dynamicsRegularizationPriorExpectedValue();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorDynSize, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinearAccelerometerMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::GyroscopeMeasurements * >(argp3);
+  result = (bool)(arg1)->updateFilterWithMeasurements((iDynTree::LinearAccelerometerMeasurements const &)*arg2,(iDynTree::GyroscopeMeasurements const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76942,23 +81936,56 @@ int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_0(
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::LinearAccelerometerMeasurements *arg2 = 0 ;
+  iDynTree::GyroscopeMeasurements *arg3 = 0 ;
+  iDynTree::MagnetometerMeasurements *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
-  iDynTree::VectorDynSize *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_updateFilterWithMeasurements",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::VectorDynSize *) &(arg1)->dynamicsRegularizationPriorExpectedValue();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorDynSize, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "2"" of type '" "iDynTree::LinearAccelerometerMeasurements const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinearAccelerometerMeasurements * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "3"" of type '" "iDynTree::GyroscopeMeasurements const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::GyroscopeMeasurements * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "4"" of type '" "iDynTree::MagnetometerMeasurements const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_updateFilterWithMeasurements" "', argument " "4"" of type '" "iDynTree::MagnetometerMeasurements const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::MagnetometerMeasurements * >(argp4);
+  result = (bool)(arg1)->updateFilterWithMeasurements((iDynTree::LinearAccelerometerMeasurements const &)*arg2,(iDynTree::GyroscopeMeasurements const &)*arg3,(iDynTree::MagnetometerMeasurements const &)*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -76966,51 +81993,76 @@ int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_1(
 }
 
 
-int _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
+int _wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_1(resc,resv,argc,argv);
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements__SWIG_0(resc,resv,argc,argv);
+        }
+      }
     }
   }
-  if (argc == 1) {
+  if (argc == 4) {
     int _v;
     void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0);
     _v = SWIG_CheckState(res);
     if (_v) {
-      return _wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue__SWIG_0(resc,resv,argc,argv);
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
     }
   }
   
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue'."
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'AttitudeQuaternionEKF_updateFilterWithMeasurements'."
     "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorExpectedValue() const\n"
-    "    iDynTree::BerdySparseMAPSolver::dynamicsRegularizationPriorExpectedValue()\n");
+    "    iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(iDynTree::LinearAccelerometerMeasurements const &,iDynTree::GyroscopeMeasurements const &)\n"
+    "    iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(iDynTree::LinearAccelerometerMeasurements const &,iDynTree::GyroscopeMeasurements const &,iDynTree::MagnetometerMeasurements const &)\n");
   return 1;
 }
 
 
-int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_propagateStates(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_measurementsPriorCovarianceInverse",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_propagateStates",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_measurementsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_propagateStates" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->measurementsPriorCovarianceInverse();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  result = (bool)(arg1)->propagateStates();
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77018,23 +82070,34 @@ int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_0(int re
 }
 
 
-int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::Rotation *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
-  iDynTree::SparseMatrix< iDynTree::ColumnMajor > *result = 0 ;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_measurementsPriorCovarianceInverse",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_measurementsPriorCovarianceInverse" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::SparseMatrix< iDynTree::ColumnMajor > *) &(arg1)->measurementsPriorCovarianceInverse();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Rotation,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix" "', argument " "2"" of type '" "iDynTree::Rotation &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix" "', argument " "2"" of type '" "iDynTree::Rotation &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Rotation * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsRotationMatrix(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77042,50 +82105,68 @@ int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_1(int re
 }
 
 
-int _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_1(resc,resv,argc,argv);
-    }
+int _wrap_AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::UnitQuaternion *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion",argc,2,2,0)) {
+    SWIG_fail;
   }
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse__SWIG_0(resc,resv,argc,argv);
-    }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_measurementsPriorCovarianceInverse'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdySparseMAPSolver::measurementsPriorCovarianceInverse() const\n"
-    "    iDynTree::BerdySparseMAPSolver::measurementsPriorCovarianceInverse()\n");
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion" "', argument " "2"" of type '" "iDynTree::UnitQuaternion &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion" "', argument " "2"" of type '" "iDynTree::UnitQuaternion &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::UnitQuaternion * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsQuaternion(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
   return 1;
 }
 
 
-int _wrap_BerdySparseMAPSolver_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRPY(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::RPY *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_isValid",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getOrientationEstimateAsRPY",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_isValid" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsRPY" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (bool)(arg1)->isValid();
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsRPY" "', argument " "2"" of type '" "iDynTree::RPY &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_getOrientationEstimateAsRPY" "', argument " "2"" of type '" "iDynTree::RPY &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::RPY * >(argp2);
+  result = (bool)(arg1)->getOrientationEstimateAsRPY(*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -77094,23 +82175,23 @@ int _wrap_BerdySparseMAPSolver_isValid(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
-int _wrap_BerdySparseMAPSolver_initialize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_getInternalStateSize(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_initialize",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getInternalStateSize",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_initialize" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getInternalStateSize" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (bool)(arg1)->initialize();
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  result = ((iDynTree::AttitudeQuaternionEKF const *)arg1)->getInternalStateSize();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77118,74 +82199,34 @@ int _wrap_BerdySparseMAPSolver_initialize(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::FrameIndex arg4 ;
-  iDynTree::Vector3 *arg5 = 0 ;
-  iDynTree::VectorDynSize *arg6 = 0 ;
+int _wrap_AttitudeQuaternionEKF_getInternalState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_updateEstimateInformationFixedBase",argc,6,6,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getInternalState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getInternalState" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_getInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_getInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::FrameIndex >(val4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFixedBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
-  (arg1)->updateEstimateInformationFixedBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,arg4,(iDynTree::Vector3 const &)*arg5,(iDynTree::VectorDynSize const &)*arg6);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::AttitudeQuaternionEKF const *)arg1)->getInternalState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77193,74 +82234,34 @@ int _wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(int resc, mxAr
 }
 
 
-int _wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::JointPosDoubleArray *arg2 = 0 ;
-  iDynTree::JointDOFsDoubleArray *arg3 = 0 ;
-  iDynTree::FrameIndex arg4 ;
-  iDynTree::Vector3 *arg5 = 0 ;
-  iDynTree::VectorDynSize *arg6 = 0 ;
+int _wrap_AttitudeQuaternionEKF_getDefaultInternalInitialState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   void *argp2 ;
   int res2 = 0 ;
-  void *argp3 ;
-  int res3 = 0 ;
-  int val4 ;
-  int ecode4 = 0 ;
-  void *argp5 ;
-  int res5 = 0 ;
-  void *argp6 ;
-  int res6 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_updateEstimateInformationFloatingBase",argc,6,6,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_getDefaultInternalInitialState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_getDefaultInternalInitialState" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__JointPosDoubleArray,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_getDefaultInternalInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "2"" of type '" "iDynTree::JointPosDoubleArray const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::JointPosDoubleArray * >(argp2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__JointDOFsDoubleArray,  0 );
-  if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "3"" of type '" "iDynTree::JointDOFsDoubleArray const &""'"); 
-  }
-  arg3 = reinterpret_cast< iDynTree::JointDOFsDoubleArray * >(argp3);
-  ecode4 = SWIG_AsVal_int(argv[3], &val4);
-  if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "4"" of type '" "iDynTree::FrameIndex""'");
-  } 
-  arg4 = static_cast< iDynTree::FrameIndex >(val4);
-  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
-  if (!SWIG_IsOK(res5)) {
-    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  if (!argp5) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "5"" of type '" "iDynTree::Vector3 const &""'"); 
-  }
-  arg5 = reinterpret_cast< iDynTree::Vector3 * >(argp5);
-  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res6)) {
-    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp6) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_updateEstimateInformationFloatingBase" "', argument " "6"" of type '" "iDynTree::VectorDynSize const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_getDefaultInternalInitialState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg6 = reinterpret_cast< iDynTree::VectorDynSize * >(argp6);
-  (arg1)->updateEstimateInformationFloatingBase((iDynTree::JointPosDoubleArray const &)*arg2,(iDynTree::JointDOFsDoubleArray const &)*arg3,arg4,(iDynTree::Vector3 const &)*arg5,(iDynTree::VectorDynSize const &)*arg6);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)((iDynTree::AttitudeQuaternionEKF const *)arg1)->getDefaultInternalInitialState((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77268,22 +82269,33 @@ int _wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(int resc, m
 }
 
 
-int _wrap_BerdySparseMAPSolver_doEstimate(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_AttitudeQuaternionEKF_setInternalState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_doEstimate",argc,1,1,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setInternalState",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_doEstimate" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setInternalState" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (bool)(arg1)->doEstimate();
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_setInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_setInternalState" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->setInternalState((iDynTree::Span< double,-1 > const &)*arg2);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -77292,33 +82304,34 @@ int _wrap_BerdySparseMAPSolver_doEstimate(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
+int _wrap_AttitudeQuaternionEKF_setInternalStateInitialOrientation(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
+  iDynTree::Span< double,-1 > *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  void *argp2 = 0 ;
+  void *argp2 ;
   int res2 = 0 ;
   mxArray * _out;
+  bool result;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_getLastEstimate",argc,2,2,0)) {
+  if (!SWIG_check_num_args("AttitudeQuaternionEKF_setInternalStateInitialOrientation",argc,2,2,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeQuaternionEKF_setInternalStateInitialOrientation" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
   if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeQuaternionEKF_setInternalStateInitialOrientation" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
   if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "AttitudeQuaternionEKF_setInternalStateInitialOrientation" "', argument " "2"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
   }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  ((iDynTree::BerdySparseMAPSolver const *)arg1)->getLastEstimate(*arg2);
-  _out = (mxArray*)0;
+  arg2 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp2);
+  result = (bool)(arg1)->setInternalStateInitialOrientation((iDynTree::Span< double,-1 > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77326,23 +82339,26 @@ int _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_0(int resc, mxArray *resv[]
 }
 
 
-int _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::BerdySparseMAPSolver *arg1 = (iDynTree::BerdySparseMAPSolver *) 0 ;
+int _wrap_delete_AttitudeQuaternionEKF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeQuaternionEKF *arg1 = (iDynTree::AttitudeQuaternionEKF *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
   mxArray * _out;
-  iDynTree::VectorDynSize *result = 0 ;
   
-  if (!SWIG_check_num_args("BerdySparseMAPSolver_getLastEstimate",argc,1,1,0)) {
+  int is_owned;
+  if (!SWIG_check_num_args("delete_AttitudeQuaternionEKF",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0 |  0 );
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeQuaternionEKF, SWIG_POINTER_DISOWN |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "BerdySparseMAPSolver_getLastEstimate" "', argument " "1"" of type '" "iDynTree::BerdySparseMAPSolver const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeQuaternionEKF" "', argument " "1"" of type '" "iDynTree::AttitudeQuaternionEKF *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::BerdySparseMAPSolver * >(argp1);
-  result = (iDynTree::VectorDynSize *) &((iDynTree::BerdySparseMAPSolver const *)arg1)->getLastEstimate();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorDynSize, 0 |  0 );
+  arg1 = reinterpret_cast< iDynTree::AttitudeQuaternionEKF * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -77350,39 +82366,6 @@ int _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_1(int resc, mxArray *resv[]
 }
 
 
-int _wrap_BerdySparseMAPSolver_getLastEstimate(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  if (argc == 1) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      return _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_1(resc,resv,argc,argv);
-    }
-  }
-  if (argc == 2) {
-    int _v;
-    void *vptr = 0;
-    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__BerdySparseMAPSolver, 0);
-    _v = SWIG_CheckState(res);
-    if (_v) {
-      void *vptr = 0;
-      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
-      _v = SWIG_CheckState(res);
-      if (_v) {
-        return _wrap_BerdySparseMAPSolver_getLastEstimate__SWIG_0(resc,resv,argc,argv);
-      }
-    }
-  }
-  
-  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'BerdySparseMAPSolver_getLastEstimate'."
-    "  Possible C/C++ prototypes are:\n"
-    "    iDynTree::BerdySparseMAPSolver::getLastEstimate(iDynTree::VectorDynSize &) const\n"
-    "    iDynTree::BerdySparseMAPSolver::getLastEstimate() const\n");
-  return 1;
-}
-
-
 int _wrap_DynamicsRegressorParameter_category_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Regressors::DynamicsRegressorParameter *arg1 = (iDynTree::Regressors::DynamicsRegressorParameter *) 0 ;
   iDynTree::Regressors::DynamicsRegressorParameterCategory arg2 ;
@@ -81761,6 +86744,179 @@ int _wrap_KinDynComputations_getFrameVel(int resc, mxArray *resv[], int argc, mx
 }
 
 
+int _wrap_KinDynComputations_getFrameAcc__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::KinDynComputations *arg1 = (iDynTree::KinDynComputations *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::Vector6 *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  iDynTree::Vector6 result;
+  
+  if (!SWIG_check_num_args("KinDynComputations_getFrameAcc",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__KinDynComputations, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "KinDynComputations_getFrameAcc" "', argument " "1"" of type '" "iDynTree::KinDynComputations *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::KinDynComputations * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "KinDynComputations_getFrameAcc" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_getFrameAcc" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "KinDynComputations_getFrameAcc" "', argument " "3"" of type '" "iDynTree::Vector6 const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_getFrameAcc" "', argument " "3"" of type '" "iDynTree::Vector6 const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Vector6 * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "KinDynComputations_getFrameAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_getFrameAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  result = (arg1)->getFrameAcc((std::string const &)*arg2,(iDynTree::Vector6 const &)*arg3,(iDynTree::VectorDynSize const &)*arg4);
+  _out = SWIG_NewPointerObj((new iDynTree::Vector6(static_cast< const iDynTree::Vector6& >(result))), SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_KinDynComputations_getFrameAcc__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::KinDynComputations *arg1 = (iDynTree::KinDynComputations *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  iDynTree::Vector6 *arg3 = 0 ;
+  iDynTree::VectorDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  iDynTree::Vector6 result;
+  
+  if (!SWIG_check_num_args("KinDynComputations_getFrameAcc",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__KinDynComputations, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "KinDynComputations_getFrameAcc" "', argument " "1"" of type '" "iDynTree::KinDynComputations *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::KinDynComputations * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "KinDynComputations_getFrameAcc" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "KinDynComputations_getFrameAcc" "', argument " "3"" of type '" "iDynTree::Vector6 const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_getFrameAcc" "', argument " "3"" of type '" "iDynTree::Vector6 const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Vector6 * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "KinDynComputations_getFrameAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_getFrameAcc" "', argument " "4"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::VectorDynSize * >(argp4);
+  result = (arg1)->getFrameAcc(arg2,(iDynTree::Vector6 const &)*arg3,(iDynTree::VectorDynSize const &)*arg4);
+  _out = SWIG_NewPointerObj((new iDynTree::Vector6(static_cast< const iDynTree::Vector6& >(result))), SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_KinDynComputations_getFrameAcc(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__KinDynComputations, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_KinDynComputations_getFrameAcc__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__KinDynComputations, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_KinDynComputations_getFrameAcc__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'KinDynComputations_getFrameAcc'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::KinDynComputations::getFrameAcc(std::string const &,iDynTree::Vector6 const &,iDynTree::VectorDynSize const &)\n"
+    "    iDynTree::KinDynComputations::getFrameAcc(iDynTree::FrameIndex const,iDynTree::Vector6 const &,iDynTree::VectorDynSize const &)\n");
+  return 1;
+}
+
+
 int _wrap_KinDynComputations_getFrameFreeFloatingJacobian__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::KinDynComputations *arg1 = (iDynTree::KinDynComputations *) 0 ;
   std::string *arg2 = 0 ;
@@ -82610,6 +87766,109 @@ int _wrap_KinDynComputations_generalizedGravityForces(int resc, mxArray *resv[],
 }
 
 
+int _wrap_KinDynComputations_generalizedExternalForces(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::KinDynComputations *arg1 = (iDynTree::KinDynComputations *) 0 ;
+  iDynTree::LinkNetExternalWrenches *arg2 = 0 ;
+  iDynTree::FreeFloatingGeneralizedTorques *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("KinDynComputations_generalizedExternalForces",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__KinDynComputations, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "KinDynComputations_generalizedExternalForces" "', argument " "1"" of type '" "iDynTree::KinDynComputations *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::KinDynComputations * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__LinkWrenches,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "KinDynComputations_generalizedExternalForces" "', argument " "2"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_generalizedExternalForces" "', argument " "2"" of type '" "iDynTree::LinkNetExternalWrenches const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::LinkNetExternalWrenches * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__FreeFloatingGeneralizedTorques,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "KinDynComputations_generalizedExternalForces" "', argument " "3"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_generalizedExternalForces" "', argument " "3"" of type '" "iDynTree::FreeFloatingGeneralizedTorques &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::FreeFloatingGeneralizedTorques * >(argp3);
+  result = (bool)(arg1)->generalizedExternalForces((iDynTree::LinkNetExternalWrenches const &)*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_KinDynComputations_inverseDynamicsInertialParametersRegressor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::KinDynComputations *arg1 = (iDynTree::KinDynComputations *) 0 ;
+  iDynTree::Vector6 *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::MatrixDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("KinDynComputations_inverseDynamicsInertialParametersRegressor",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__KinDynComputations, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "1"" of type '" "iDynTree::KinDynComputations *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::KinDynComputations * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "2"" of type '" "iDynTree::Vector6 const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "2"" of type '" "iDynTree::Vector6 const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Vector6 * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "KinDynComputations_inverseDynamicsInertialParametersRegressor" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp4);
+  result = (bool)(arg1)->inverseDynamicsInertialParametersRegressor((iDynTree::Vector6 const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_delete_ICamera(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::ICamera *arg1 = (iDynTree::ICamera *) 0 ;
   void *argp1 = 0 ;
@@ -83793,60 +89052,600 @@ int _wrap_IEnvironment_removeLight(int resc, mxArray *resv[], int argc, mxArray
   std::string *arg2 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IEnvironment_removeLight",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IEnvironment, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IEnvironment_removeLight" "', argument " "1"" of type '" "iDynTree::IEnvironment *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IEnvironment * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IEnvironment_removeLight" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IEnvironment_removeLight" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->removeLight((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_delete_IJetsVisualization(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_IJetsVisualization",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_IJetsVisualization" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_setJetsFrames(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  std::vector< std::string,std::allocator< std::string > > *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_setJetsFrames",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetsFrames" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  {
+    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
+    res2 = swig::asptr(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJetsVisualization_setJetsFrames" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetsFrames" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->setJetsFrames((std::vector< std::string,std::allocator< std::string > > const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_getNrOfJets(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_getNrOfJets",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_getNrOfJets" "', argument " "1"" of type '" "iDynTree::IJetsVisualization const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  result = ((iDynTree::IJetsVisualization const *)arg1)->getNrOfJets();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_getJetDirection(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  int arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  iDynTree::Direction result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_getJetDirection",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_getJetDirection" "', argument " "1"" of type '" "iDynTree::IJetsVisualization const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_getJetDirection" "', argument " "2"" of type '" "int""'");
+  } 
+  arg2 = static_cast< int >(val2);
+  result = ((iDynTree::IJetsVisualization const *)arg1)->getJetDirection(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Direction(static_cast< const iDynTree::Direction& >(result))), SWIGTYPE_p_iDynTree__Direction, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_setJetDirection(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  int arg2 ;
+  iDynTree::Direction *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_setJetDirection",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetDirection" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_setJetDirection" "', argument " "2"" of type '" "int""'");
+  } 
+  arg2 = static_cast< int >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJetsVisualization_setJetDirection" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetDirection" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
+  result = (bool)(arg1)->setJetDirection(arg2,(iDynTree::Direction const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_setJetColor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  int arg2 ;
+  iDynTree::ColorViz *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_setJetColor",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetColor" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_setJetColor" "', argument " "2"" of type '" "int""'");
+  } 
+  arg2 = static_cast< int >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__ColorViz,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJetsVisualization_setJetColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::ColorViz * >(argp3);
+  result = (bool)(arg1)->setJetColor(arg2,(iDynTree::ColorViz const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_setJetsDimensions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  double *arg2 = 0 ;
+  double *arg3 = 0 ;
+  double *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  double temp2 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double temp3 ;
+  double val3 ;
+  int ecode3 = 0 ;
+  double temp4 ;
+  double val4 ;
+  int ecode4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_setJetsDimensions",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "2"" of type '" "double""'");
+  } 
+  temp2 = static_cast< double >(val2);
+  arg2 = &temp2;
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "3"" of type '" "double""'");
+  } 
+  temp3 = static_cast< double >(val3);
+  arg3 = &temp3;
+  ecode4 = SWIG_AsVal_double(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "4"" of type '" "double""'");
+  } 
+  temp4 = static_cast< double >(val4);
+  arg4 = &temp4;
+  result = (bool)(arg1)->setJetsDimensions((double const &)*arg2,(double const &)*arg3,(double const &)*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IJetsVisualization_setJetsIntensity(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IJetsVisualization_setJetsIntensity",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetsIntensity" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJetsVisualization_setJetsIntensity" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetsIntensity" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  result = (bool)(arg1)->setJetsIntensity((iDynTree::VectorDynSize const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_IVectorsVisualization(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_IVectorsVisualization",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_IVectorsVisualization" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IVectorsVisualization_addVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  iDynTree::Position *arg2 = 0 ;
+  iDynTree::Direction *arg3 = 0 ;
+  double arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  double val4 ;
+  int ecode4 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("IVectorsVisualization_addVector",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_addVector" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IVectorsVisualization_addVector" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_addVector" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_addVector" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_addVector" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
+  ecode4 = SWIG_AsVal_double(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IVectorsVisualization_addVector" "', argument " "4"" of type '" "double""'");
+  } 
+  arg4 = static_cast< double >(val4);
+  result = (arg1)->addVector((iDynTree::Position const &)*arg2,(iDynTree::Direction const &)*arg3,arg4);
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IVectorsVisualization_addVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  iDynTree::Position *arg2 = 0 ;
+  iDynTree::Vector3 *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  size_t result;
+  
+  if (!SWIG_check_num_args("IVectorsVisualization_addVector",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_addVector" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IVectorsVisualization_addVector" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_addVector" "', argument " "2"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Position * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_addVector" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_addVector" "', argument " "3"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Vector3 * >(argp3);
+  result = (arg1)->addVector((iDynTree::Position const &)*arg2,(iDynTree::Vector3 const &)*arg3);
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IVectorsVisualization_addVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IVectorsVisualization, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_IVectorsVisualization_addVector__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IVectorsVisualization, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          {
+            int res = SWIG_AsVal_double(argv[3], NULL);
+            _v = SWIG_CheckState(res);
+          }
+          if (_v) {
+            return _wrap_IVectorsVisualization_addVector__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'IVectorsVisualization_addVector'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::IVectorsVisualization::addVector(iDynTree::Position const &,iDynTree::Direction const &,double)\n"
+    "    iDynTree::IVectorsVisualization::addVector(iDynTree::Position const &,iDynTree::Vector3 const &)\n");
+  return 1;
+}
+
+
+int _wrap_IVectorsVisualization_getNrOfVectors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
   mxArray * _out;
-  bool result;
+  size_t result;
   
-  if (!SWIG_check_num_args("IEnvironment_removeLight",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_getNrOfVectors",argc,1,1,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IEnvironment, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IEnvironment_removeLight" "', argument " "1"" of type '" "iDynTree::IEnvironment *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IEnvironment * >(argp1);
-  {
-    std::string *ptr = (std::string *)0;
-    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IEnvironment_removeLight" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IEnvironment_removeLight" "', argument " "2"" of type '" "std::string const &""'"); 
-    }
-    arg2 = ptr;
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_getNrOfVectors" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization const *""'"); 
   }
-  result = (bool)(arg1)->removeLight((std::string const &)*arg2);
-  _out = SWIG_From_bool(static_cast< bool >(result));
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  result = ((iDynTree::IVectorsVisualization const *)arg1)->getNrOfVectors();
+  _out = SWIG_From_size_t(static_cast< size_t >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_delete_IJetsVisualization(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
+int _wrap_IVectorsVisualization_getVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  size_t arg2 ;
+  iDynTree::Position *arg3 = 0 ;
+  iDynTree::Direction *arg4 = 0 ;
+  double *arg5 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  void *argp5 = 0 ;
+  int res5 = 0 ;
   mxArray * _out;
+  bool result;
   
-  int is_owned;
-  if (!SWIG_check_num_args("delete_IJetsVisualization",argc,1,1,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_getVector",argc,5,5,0)) {
     SWIG_fail;
   }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, SWIG_POINTER_DISOWN |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_IJetsVisualization" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_getVector" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  if (is_owned) {
-    delete arg1;
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IVectorsVisualization_getVector" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_getVector" "', argument " "3"" of type '" "iDynTree::Position &""'"); 
   }
-  _out = (mxArray*)0;
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_getVector" "', argument " "3"" of type '" "iDynTree::Position &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Position * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IVectorsVisualization_getVector" "', argument " "4"" of type '" "iDynTree::Direction &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_getVector" "', argument " "4"" of type '" "iDynTree::Direction &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Direction * >(argp4);
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_double,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "IVectorsVisualization_getVector" "', argument " "5"" of type '" "double &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_getVector" "', argument " "5"" of type '" "double &""'"); 
+  }
+  arg5 = reinterpret_cast< double * >(argp5);
+  result = (bool)((iDynTree::IVectorsVisualization const *)arg1)->getVector(arg2,*arg3,*arg4,*arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -83854,94 +89653,180 @@ int _wrap_delete_IJetsVisualization(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
-int _wrap_IJetsVisualization_setJetsFrames(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  std::vector< std::string,std::allocator< std::string > > *arg2 = 0 ;
+int _wrap_IVectorsVisualization_getVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  size_t arg2 ;
+  iDynTree::Position *arg3 = 0 ;
+  iDynTree::Vector3 *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int res2 = SWIG_OLDOBJ ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("IJetsVisualization_setJetsFrames",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_getVector",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetsFrames" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_getVector" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization const *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  {
-    std::vector< std::string,std::allocator< std::string > > *ptr = (std::vector< std::string,std::allocator< std::string > > *)0;
-    res2 = swig::asptr(argv[1], &ptr);
-    if (!SWIG_IsOK(res2)) {
-      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJetsVisualization_setJetsFrames" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    if (!ptr) {
-      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetsFrames" "', argument " "2"" of type '" "std::vector< std::string,std::allocator< std::string > > const &""'"); 
-    }
-    arg2 = ptr;
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IVectorsVisualization_getVector" "', argument " "2"" of type '" "size_t""'");
+  } 
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_getVector" "', argument " "3"" of type '" "iDynTree::Position &""'"); 
   }
-  result = (bool)(arg1)->setJetsFrames((std::vector< std::string,std::allocator< std::string > > const &)*arg2);
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_getVector" "', argument " "3"" of type '" "iDynTree::Position &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Position * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IVectorsVisualization_getVector" "', argument " "4"" of type '" "iDynTree::Vector3 &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_getVector" "', argument " "4"" of type '" "iDynTree::Vector3 &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Vector3 * >(argp4);
+  result = (bool)((iDynTree::IVectorsVisualization const *)arg1)->getVector(arg2,*arg3,*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 0;
 fail:
-  if (SWIG_IsNewObj(res2)) delete arg2;
   return 1;
 }
 
 
-int _wrap_IJetsVisualization_getNrOfJets(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  size_t result;
-  
-  if (!SWIG_check_num_args("IJetsVisualization_getNrOfJets",argc,1,1,0)) {
-    SWIG_fail;
+int _wrap_IVectorsVisualization_getVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IVectorsVisualization, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_IVectorsVisualization_getVector__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_getNrOfJets" "', argument " "1"" of type '" "iDynTree::IJetsVisualization const *""'"); 
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IVectorsVisualization, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_double, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_IVectorsVisualization_getVector__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  result = ((iDynTree::IJetsVisualization const *)arg1)->getNrOfJets();
-  _out = SWIG_From_size_t(static_cast< size_t >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'IVectorsVisualization_getVector'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::IVectorsVisualization::getVector(size_t,iDynTree::Position &,iDynTree::Direction &,double &) const\n"
+    "    iDynTree::IVectorsVisualization::getVector(size_t,iDynTree::Position &,iDynTree::Vector3 &) const\n");
   return 1;
 }
 
 
-int _wrap_IJetsVisualization_getJetDirection(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  int arg2 ;
+int _wrap_IVectorsVisualization_updateVector__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  size_t arg2 ;
+  iDynTree::Position *arg3 = 0 ;
+  iDynTree::Direction *arg4 = 0 ;
+  double arg5 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  size_t val2 ;
   int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  double val5 ;
+  int ecode5 = 0 ;
   mxArray * _out;
-  iDynTree::Direction result;
+  bool result;
   
-  if (!SWIG_check_num_args("IJetsVisualization_getJetDirection",argc,2,2,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_updateVector",argc,5,5,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_getJetDirection" "', argument " "1"" of type '" "iDynTree::IJetsVisualization const *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_updateVector" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_getJetDirection" "', argument " "2"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IVectorsVisualization_updateVector" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg2 = static_cast< int >(val2);
-  result = ((iDynTree::IJetsVisualization const *)arg1)->getJetDirection(arg2);
-  _out = SWIG_NewPointerObj((new iDynTree::Direction(static_cast< const iDynTree::Direction& >(result))), SWIGTYPE_p_iDynTree__Direction, SWIG_POINTER_OWN |  0 );
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Position,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_updateVector" "', argument " "3"" of type '" "iDynTree::Position const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_updateVector" "', argument " "3"" of type '" "iDynTree::Position const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Position * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__Direction,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IVectorsVisualization_updateVector" "', argument " "4"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_updateVector" "', argument " "4"" of type '" "iDynTree::Direction const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Direction * >(argp4);
+  ecode5 = SWIG_AsVal_double(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "IVectorsVisualization_updateVector" "', argument " "5"" of type '" "double""'");
+  } 
+  arg5 = static_cast< double >(val5);
+  result = (bool)(arg1)->updateVector(arg2,(iDynTree::Position const &)*arg3,(iDynTree::Direction const &)*arg4,arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
 fail:
@@ -83949,41 +89834,52 @@ int _wrap_IJetsVisualization_getJetDirection(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_IJetsVisualization_setJetDirection(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  int arg2 ;
-  iDynTree::Direction *arg3 = 0 ;
+int _wrap_IVectorsVisualization_updateVector__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  size_t arg2 ;
+  iDynTree::Position *arg3 = 0 ;
+  iDynTree::Vector3 *arg4 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  size_t val2 ;
   int ecode2 = 0 ;
   void *argp3 ;
   int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("IJetsVisualization_setJetDirection",argc,3,3,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_updateVector",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetDirection" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_updateVector" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_setJetDirection" "', argument " "2"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IVectorsVisualization_updateVector" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg2 = static_cast< int >(val2);
-  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Direction,  0 );
+  arg2 = static_cast< size_t >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Position,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJetsVisualization_setJetDirection" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_updateVector" "', argument " "3"" of type '" "iDynTree::Position const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetDirection" "', argument " "3"" of type '" "iDynTree::Direction const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_updateVector" "', argument " "3"" of type '" "iDynTree::Position const &""'"); 
   }
-  arg3 = reinterpret_cast< iDynTree::Direction * >(argp3);
-  result = (bool)(arg1)->setJetDirection(arg2,(iDynTree::Direction const &)*arg3);
+  arg3 = reinterpret_cast< iDynTree::Position * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "IVectorsVisualization_updateVector" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_updateVector" "', argument " "4"" of type '" "iDynTree::Vector3 const &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::Vector3 * >(argp4);
+  result = (bool)(arg1)->updateVector(arg2,(iDynTree::Position const &)*arg3,(iDynTree::Vector3 const &)*arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -83992,41 +89888,107 @@ int _wrap_IJetsVisualization_setJetDirection(int resc, mxArray *resv[], int argc
 }
 
 
-int _wrap_IJetsVisualization_setJetColor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  int arg2 ;
+int _wrap_IVectorsVisualization_updateVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IVectorsVisualization, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_IVectorsVisualization_updateVector__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__IVectorsVisualization, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Position, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__Direction, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            {
+              int res = SWIG_AsVal_double(argv[4], NULL);
+              _v = SWIG_CheckState(res);
+            }
+            if (_v) {
+              return _wrap_IVectorsVisualization_updateVector__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'IVectorsVisualization_updateVector'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::IVectorsVisualization::updateVector(size_t,iDynTree::Position const &,iDynTree::Direction const &,double)\n"
+    "    iDynTree::IVectorsVisualization::updateVector(size_t,iDynTree::Position const &,iDynTree::Vector3 const &)\n");
+  return 1;
+}
+
+
+int _wrap_IVectorsVisualization_setVectorColor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  size_t arg2 ;
   iDynTree::ColorViz *arg3 = 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  int val2 ;
+  size_t val2 ;
   int ecode2 = 0 ;
   void *argp3 ;
   int res3 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("IJetsVisualization_setJetColor",argc,3,3,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_setVectorColor",argc,3,3,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetColor" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_setVectorColor" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_setJetColor" "', argument " "2"" of type '" "int""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IVectorsVisualization_setVectorColor" "', argument " "2"" of type '" "size_t""'");
   } 
-  arg2 = static_cast< int >(val2);
+  arg2 = static_cast< size_t >(val2);
   res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__ColorViz,  0 );
   if (!SWIG_IsOK(res3)) {
-    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IJetsVisualization_setJetColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IVectorsVisualization_setVectorColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
   }
   if (!argp3) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IVectorsVisualization_setVectorColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
   }
   arg3 = reinterpret_cast< iDynTree::ColorViz * >(argp3);
-  result = (bool)(arg1)->setJetColor(arg2,(iDynTree::ColorViz const &)*arg3);
+  result = (bool)(arg1)->setVectorColor(arg2,(iDynTree::ColorViz const &)*arg3);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -84035,87 +89997,46 @@ int _wrap_IJetsVisualization_setJetColor(int resc, mxArray *resv[], int argc, mx
 }
 
 
-int _wrap_IJetsVisualization_setJetsDimensions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  double *arg2 = 0 ;
-  double *arg3 = 0 ;
-  double *arg4 = 0 ;
+int _wrap_IVectorsVisualization_setVectorsAspect(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IVectorsVisualization *arg1 = (iDynTree::IVectorsVisualization *) 0 ;
+  double arg2 ;
+  double arg3 ;
+  double arg4 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
-  double temp2 ;
   double val2 ;
   int ecode2 = 0 ;
-  double temp3 ;
   double val3 ;
   int ecode3 = 0 ;
-  double temp4 ;
   double val4 ;
   int ecode4 = 0 ;
   mxArray * _out;
   bool result;
   
-  if (!SWIG_check_num_args("IJetsVisualization_setJetsDimensions",argc,4,4,0)) {
+  if (!SWIG_check_num_args("IVectorsVisualization_setVectorsAspect",argc,4,4,0)) {
     SWIG_fail;
   }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
   if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IVectorsVisualization_setVectorsAspect" "', argument " "1"" of type '" "iDynTree::IVectorsVisualization *""'"); 
   }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
+  arg1 = reinterpret_cast< iDynTree::IVectorsVisualization * >(argp1);
   ecode2 = SWIG_AsVal_double(argv[1], &val2);
   if (!SWIG_IsOK(ecode2)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "2"" of type '" "double""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IVectorsVisualization_setVectorsAspect" "', argument " "2"" of type '" "double""'");
   } 
-  temp2 = static_cast< double >(val2);
-  arg2 = &temp2;
+  arg2 = static_cast< double >(val2);
   ecode3 = SWIG_AsVal_double(argv[2], &val3);
   if (!SWIG_IsOK(ecode3)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "3"" of type '" "double""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "IVectorsVisualization_setVectorsAspect" "', argument " "3"" of type '" "double""'");
   } 
-  temp3 = static_cast< double >(val3);
-  arg3 = &temp3;
+  arg3 = static_cast< double >(val3);
   ecode4 = SWIG_AsVal_double(argv[3], &val4);
   if (!SWIG_IsOK(ecode4)) {
-    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IJetsVisualization_setJetsDimensions" "', argument " "4"" of type '" "double""'");
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "IVectorsVisualization_setVectorsAspect" "', argument " "4"" of type '" "double""'");
   } 
-  temp4 = static_cast< double >(val4);
-  arg4 = &temp4;
-  result = (bool)(arg1)->setJetsDimensions((double const &)*arg2,(double const &)*arg3,(double const &)*arg4);
-  _out = SWIG_From_bool(static_cast< bool >(result));
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_IJetsVisualization_setJetsIntensity(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::IJetsVisualization *arg1 = (iDynTree::IJetsVisualization *) 0 ;
-  iDynTree::VectorDynSize *arg2 = 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  bool result;
-  
-  if (!SWIG_check_num_args("IJetsVisualization_setJetsIntensity",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IJetsVisualization, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IJetsVisualization_setJetsIntensity" "', argument " "1"" of type '" "iDynTree::IJetsVisualization *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::IJetsVisualization * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "IJetsVisualization_setJetsIntensity" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  if (!argp2) {
-    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IJetsVisualization_setJetsIntensity" "', argument " "2"" of type '" "iDynTree::VectorDynSize const &""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
-  result = (bool)(arg1)->setJetsIntensity((iDynTree::VectorDynSize const &)*arg2);
+  arg4 = static_cast< double >(val4);
+  result = (bool)(arg1)->setVectorsAspect(arg2,arg3,arg4);
   _out = SWIG_From_bool(static_cast< bool >(result));
   if (_out) --resc, *resv++ = _out;
   return 0;
@@ -84368,6 +90289,85 @@ int _wrap_IModelVisualization_resetModelColor(int resc, mxArray *resv[], int arg
 }
 
 
+int _wrap_IModelVisualization_setLinkColor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IModelVisualization *arg1 = (iDynTree::IModelVisualization *) 0 ;
+  iDynTree::LinkIndex *arg2 = 0 ;
+  iDynTree::ColorViz *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  iDynTree::LinkIndex temp2 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IModelVisualization_setLinkColor",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IModelVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IModelVisualization_setLinkColor" "', argument " "1"" of type '" "iDynTree::IModelVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IModelVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IModelVisualization_setLinkColor" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  temp2 = static_cast< iDynTree::LinkIndex >(val2);
+  arg2 = &temp2;
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__ColorViz,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "IModelVisualization_setLinkColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "IModelVisualization_setLinkColor" "', argument " "3"" of type '" "iDynTree::ColorViz const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::ColorViz * >(argp3);
+  result = (bool)(arg1)->setLinkColor((iDynTree::LinkIndex const &)*arg2,(iDynTree::ColorViz const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IModelVisualization_resetLinkColor(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IModelVisualization *arg1 = (iDynTree::IModelVisualization *) 0 ;
+  iDynTree::LinkIndex *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  iDynTree::LinkIndex temp2 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("IModelVisualization_resetLinkColor",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IModelVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IModelVisualization_resetLinkColor" "', argument " "1"" of type '" "iDynTree::IModelVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IModelVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IModelVisualization_resetLinkColor" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  temp2 = static_cast< iDynTree::LinkIndex >(val2);
+  arg2 = &temp2;
+  result = (bool)(arg1)->resetLinkColor((iDynTree::LinkIndex const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_IModelVisualization_getLinkNames(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::IModelVisualization *arg1 = (iDynTree::IModelVisualization *) 0 ;
   void *argp1 = 0 ;
@@ -84534,6 +90534,64 @@ int _wrap_IModelVisualization_jets(int resc, mxArray *resv[], int argc, mxArray
 }
 
 
+int _wrap_IModelVisualization_getWorldModelTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IModelVisualization *arg1 = (iDynTree::IModelVisualization *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Transform result;
+  
+  if (!SWIG_check_num_args("IModelVisualization_getWorldModelTransform",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IModelVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IModelVisualization_getWorldModelTransform" "', argument " "1"" of type '" "iDynTree::IModelVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IModelVisualization * >(argp1);
+  result = (arg1)->getWorldModelTransform();
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_IModelVisualization_getWorldLinkTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::IModelVisualization *arg1 = (iDynTree::IModelVisualization *) 0 ;
+  iDynTree::LinkIndex *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  iDynTree::LinkIndex temp2 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  iDynTree::Transform result;
+  
+  if (!SWIG_check_num_args("IModelVisualization_getWorldLinkTransform",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__IModelVisualization, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "IModelVisualization_getWorldLinkTransform" "', argument " "1"" of type '" "iDynTree::IModelVisualization *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::IModelVisualization * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "IModelVisualization_getWorldLinkTransform" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  temp2 = static_cast< iDynTree::LinkIndex >(val2);
+  arg2 = &temp2;
+  result = (arg1)->getWorldLinkTransform((iDynTree::LinkIndex const &)*arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_VisualizerOptions_verbose_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::VisualizerOptions *arg1 = (iDynTree::VisualizerOptions *) 0 ;
   bool arg2 ;
@@ -85234,6 +91292,30 @@ int _wrap_Visualizer_enviroment(int resc, mxArray *resv[], int argc, mxArray *ar
 }
 
 
+int _wrap_Visualizer_vectors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Visualizer *arg1 = (iDynTree::Visualizer *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::IVectorsVisualization *result = 0 ;
+  
+  if (!SWIG_check_num_args("Visualizer_vectors",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Visualizer, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Visualizer_vectors" "', argument " "1"" of type '" "iDynTree::Visualizer *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Visualizer * >(argp1);
+  result = (iDynTree::IVectorsVisualization *) &(arg1)->vectors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__IVectorsVisualization, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_Visualizer_run(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Visualizer *arg1 = (iDynTree::Visualizer *) 0 ;
   void *argp1 = 0 ;
@@ -87683,17 +93765,23 @@ int _wrap_DynamicsComputations_getCenterOfMassJacobian(int resc, mxArray *resv[]
 
 /* -------- TYPE CONVERSION AND EQUIVALENCE RULES (BEGIN) -------- */
 
-static void *_p_iDynTree__ClassicalAccTo_p_iDynTree__VectorFixSizeT_6_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::VectorFixSize< 6 > *)  ((iDynTree::ClassicalAcc *) x));
+static void *_p_iDynTree__ForceVector3T_iDynTree__LinearForceVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3< iDynTree::LinearForceVector3 > *)  ((iDynTree::ForceVector3< iDynTree::LinearForceVector3 > *) x));
 }
-static void *_p_iDynTree__RotationTo_p_iDynTree__RotationRaw(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::RotationRaw *)  ((iDynTree::Rotation *) x));
+static void *_p_iDynTree__LinearForceVector3To_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3< iDynTree::LinearForceVector3 > *) (iDynTree::ForceVector3< iDynTree::LinearForceVector3 > *) ((iDynTree::LinearForceVector3 *) x));
 }
-static void *_p_iDynTree__PositionTo_p_iDynTree__PositionRaw(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::PositionRaw *)  ((iDynTree::Position *) x));
+static void *_p_iDynTree__SpatialInertiaTo_p_iDynTree__SpatialInertiaRaw(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialInertiaRaw *)  ((iDynTree::SpatialInertia *) x));
 }
-static void *_p_iDynTree__LinearForceVector3SemanticsTo_p_iDynTree__ForceVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::ForceVector3Semantics< iDynTree::LinearForceVector3Semantics > *)  ((iDynTree::LinearForceVector3Semantics *) x));
+static void *_p_iDynTree__RotationalInertiaRawTo_p_iDynTree__MatrixFixSizeT_3_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MatrixFixSize< 3,3 > *)  ((iDynTree::RotationalInertiaRaw *) x));
+}
+static void *_p_iDynTree__RotationRawTo_p_iDynTree__MatrixFixSizeT_3_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MatrixFixSize< 3,3 > *)  ((iDynTree::RotationRaw *) x));
+}
+static void *_p_iDynTree__RotationTo_p_iDynTree__MatrixFixSizeT_3_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MatrixFixSize< 3,3 > *) (iDynTree::RotationRaw *) ((iDynTree::Rotation *) x));
 }
 static void *_p_iDynTree__RevoluteJointTo_p_iDynTree__MovableJointImplT_1_1_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::MovableJointImpl< 1,1 > *)  ((iDynTree::RevoluteJoint *) x));
@@ -87701,6 +93789,75 @@ static void *_p_iDynTree__RevoluteJointTo_p_iDynTree__MovableJointImplT_1_1_t(vo
 static void *_p_iDynTree__PrismaticJointTo_p_iDynTree__MovableJointImplT_1_1_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::MovableJointImpl< 1,1 > *)  ((iDynTree::PrismaticJoint *) x));
 }
+static void *_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3< iDynTree::LinearMotionVector3 > *)  ((iDynTree::MotionVector3< iDynTree::LinearMotionVector3 > *) x));
+}
+static void *_p_iDynTree__LinearMotionVector3To_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3< iDynTree::LinearMotionVector3 > *) (iDynTree::MotionVector3< iDynTree::LinearMotionVector3 > *) ((iDynTree::LinearMotionVector3 *) x));
+}
+static void *_p_iDynTree__AngularForceVector3SemanticsTo_p_iDynTree__ForceVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::ForceVector3Semantics< iDynTree::AngularForceVector3Semantics > *)  ((iDynTree::AngularForceVector3Semantics *) x));
+}
+static void *_p_iDynTree__LinearForceVector3SemanticsTo_p_iDynTree__ForceVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::ForceVector3Semantics< iDynTree::LinearForceVector3Semantics > *)  ((iDynTree::LinearForceVector3Semantics *) x));
+}
+static void *_p_iDynTree__AttitudeMahonyFilterTo_p_iDynTree__IAttitudeEstimator(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IAttitudeEstimator *)  ((iDynTree::AttitudeMahonyFilter *) x));
+}
+static void *_p_iDynTree__AttitudeQuaternionEKFTo_p_iDynTree__IAttitudeEstimator(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IAttitudeEstimator *)  ((iDynTree::AttitudeQuaternionEKF *) x));
+}
+static void *_p_iDynTree__LinearMotionVector3To_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MotionVector3< iDynTree::LinearMotionVector3 > *)  ((iDynTree::LinearMotionVector3 *) x));
+}
+static void *_p_iDynTree__AngularForceVector3To_p_iDynTree__ForceVector3T_iDynTree__AngularForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::ForceVector3< iDynTree::AngularForceVector3 > *)  ((iDynTree::AngularForceVector3 *) x));
+}
+static void *_p_iDynTree__MovableJointImplT_1_1_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 1,1 > *) x));
+}
+static void *_p_iDynTree__RevoluteJointTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *) (iDynTree::MovableJointImpl< 1,1 > *) ((iDynTree::RevoluteJoint *) x));
+}
+static void *_p_iDynTree__MovableJointImplT_6_6_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 6,6 > *) x));
+}
+static void *_p_iDynTree__MovableJointImplT_5_5_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 5,5 > *) x));
+}
+static void *_p_iDynTree__PrismaticJointTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *) (iDynTree::MovableJointImpl< 1,1 > *) ((iDynTree::PrismaticJoint *) x));
+}
+static void *_p_iDynTree__MovableJointImplT_4_4_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 4,4 > *) x));
+}
+static void *_p_iDynTree__FixedJointTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::FixedJoint *) x));
+}
+static void *_p_iDynTree__MovableJointImplT_3_3_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 3,3 > *) x));
+}
+static void *_p_iDynTree__MovableJointImplT_2_2_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 2,2 > *) x));
+}
+static void *_p_iDynTree__BoxTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SolidShape *)  ((iDynTree::Box *) x));
+}
+static void *_p_iDynTree__ExternalMeshTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SolidShape *)  ((iDynTree::ExternalMesh *) x));
+}
+static void *_p_iDynTree__SphereTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SolidShape *)  ((iDynTree::Sphere *) x));
+}
+static void *_p_iDynTree__CylinderTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SolidShape *)  ((iDynTree::Cylinder *) x));
+}
+static void *_p_iDynTree__PositionTo_p_iDynTree__PositionRaw(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::PositionRaw *)  ((iDynTree::Position *) x));
+}
+static void *_p_iDynTree__RotationTo_p_iDynTree__RotationRaw(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::RotationRaw *)  ((iDynTree::Rotation *) x));
+}
 static void *_p_iDynTree__DirectionTo_p_iDynTree__VectorFixSizeT_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::VectorFixSize< 3 > *)  ((iDynTree::Direction *) x));
 }
@@ -87746,29 +93903,14 @@ static void *_p_iDynTree__GeomVector3T_iDynTree__AngularMotionVector3_tTo_p_iDyn
 static void *_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_tTo_p_iDynTree__VectorFixSizeT_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::VectorFixSize< 3 > *)  ((iDynTree::GeomVector3< iDynTree::LinearForceVector3 > *) x));
 }
-static void *_p_iDynTree__FrameFreeFloatingJacobianTo_p_iDynTree__MatrixDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MatrixDynSize *)  ((iDynTree::FrameFreeFloatingJacobian *) x));
-}
-static void *_p_iDynTree__MomentumFreeFloatingJacobianTo_p_iDynTree__MatrixDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MatrixDynSize *)  ((iDynTree::MomentumFreeFloatingJacobian *) x));
-}
-static void *_p_iDynTree__FreeFloatingMassMatrixTo_p_iDynTree__MatrixDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MatrixDynSize *)  ((iDynTree::FreeFloatingMassMatrix *) x));
-}
-static void *_p_iDynTree__JointPosDoubleArrayTo_p_iDynTree__VectorDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::VectorDynSize *)  ((iDynTree::JointPosDoubleArray *) x));
-}
-static void *_p_iDynTree__JointDOFsDoubleArrayTo_p_iDynTree__VectorDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::VectorDynSize *)  ((iDynTree::JointDOFsDoubleArray *) x));
-}
-static void *_p_iDynTree__WrenchTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialForceVector > *) (iDynTree::SpatialForceVector *) ((iDynTree::Wrench *) x));
+static void *_p_iDynTree__LinearForceVector3To_p_iDynTree__ForceVector3T_iDynTree__LinearForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::ForceVector3< iDynTree::LinearForceVector3 > *)  ((iDynTree::LinearForceVector3 *) x));
 }
-static void *_p_iDynTree__SpatialForceVectorTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialForceVector > *)  ((iDynTree::SpatialForceVector *) x));
+static void *_p_iDynTree__ForceVector3T_iDynTree__AngularForceVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__AngularForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3< iDynTree::AngularForceVector3 > *)  ((iDynTree::ForceVector3< iDynTree::AngularForceVector3 > *) x));
 }
-static void *_p_iDynTree__SpatialMomentumTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialForceVector > *) (iDynTree::SpatialForceVector *) ((iDynTree::SpatialMomentum *) x));
+static void *_p_iDynTree__AngularForceVector3To_p_iDynTree__GeomVector3T_iDynTree__AngularForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3< iDynTree::AngularForceVector3 > *) (iDynTree::ForceVector3< iDynTree::AngularForceVector3 > *) ((iDynTree::AngularForceVector3 *) x));
 }
 static void *_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__AngularMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::GeomVector3< iDynTree::AngularMotionVector3 > *)  ((iDynTree::MotionVector3< iDynTree::AngularMotionVector3 > *) x));
@@ -87776,47 +93918,29 @@ static void *_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_tTo_p_iD
 static void *_p_iDynTree__AngularMotionVector3To_p_iDynTree__GeomVector3T_iDynTree__AngularMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::GeomVector3< iDynTree::AngularMotionVector3 > *) (iDynTree::MotionVector3< iDynTree::AngularMotionVector3 > *) ((iDynTree::AngularMotionVector3 *) x));
 }
-static void *_p_iDynTree__AngularForceVector3To_p_iDynTree__ForceVector3T_iDynTree__AngularForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::ForceVector3< iDynTree::AngularForceVector3 > *)  ((iDynTree::AngularForceVector3 *) x));
-}
-static void *_p_iDynTree__SixAxisForceTorqueSensorTo_p_iDynTree__JointSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::JointSensor *)  ((iDynTree::SixAxisForceTorqueSensor *) x));
-}
-static void *_p_iDynTree__JointSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *)  ((iDynTree::JointSensor *) x));
-}
-static void *_p_iDynTree__AccelerometerSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::AccelerometerSensor *) x));
-}
-static void *_p_iDynTree__ThreeAxisAngularAccelerometerSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::ThreeAxisAngularAccelerometerSensor *) x));
-}
-static void *_p_iDynTree__ThreeAxisForceTorqueContactSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::ThreeAxisForceTorqueContactSensor *) x));
-}
-static void *_p_iDynTree__LinkSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *)  ((iDynTree::LinkSensor *) x));
+static void *_p_iDynTree__LinearMotionVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearMotionVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::LinearMotionVector3Semantics > *)  ((iDynTree::LinearMotionVector3Semantics *) x));
 }
-static void *_p_iDynTree__SixAxisForceTorqueSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *) (iDynTree::JointSensor *) ((iDynTree::SixAxisForceTorqueSensor *) x));
+static void *_p_iDynTree__LinearForceVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::LinearForceVector3Semantics > *) (iDynTree::ForceVector3Semantics< iDynTree::LinearForceVector3Semantics > *) ((iDynTree::LinearForceVector3Semantics *) x));
 }
-static void *_p_iDynTree__GyroscopeSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::GyroscopeSensor *) x));
+static void *_p_iDynTree__ForceVector3SemanticsT_iDynTree__LinearForceVector3Semantics_tTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::LinearForceVector3Semantics > *)  ((iDynTree::ForceVector3Semantics< iDynTree::LinearForceVector3Semantics > *) x));
 }
-static void *_p_iDynTree__BoxTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SolidShape *)  ((iDynTree::Box *) x));
+static void *_p_iDynTree__AngularMotionVector3To_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MotionVector3< iDynTree::AngularMotionVector3 > *)  ((iDynTree::AngularMotionVector3 *) x));
 }
-static void *_p_iDynTree__ExternalMeshTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SolidShape *)  ((iDynTree::ExternalMesh *) x));
+static void *_p_iDynTree__SpatialMotionVectorTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialMotionVector > *)  ((iDynTree::SpatialMotionVector *) x));
 }
-static void *_p_iDynTree__SphereTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SolidShape *)  ((iDynTree::Sphere *) x));
+static void *_p_iDynTree__TwistTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialMotionVector > *) (iDynTree::SpatialMotionVector *) ((iDynTree::Twist *) x));
 }
-static void *_p_iDynTree__CylinderTo_p_iDynTree__SolidShape(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SolidShape *)  ((iDynTree::Cylinder *) x));
+static void *_p_iDynTree__SpatialAccTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialMotionVector > *) (iDynTree::SpatialMotionVector *) ((iDynTree::SpatialAcc *) x));
 }
-static void *_p_iDynTree__SpatialInertiaTo_p_iDynTree__SpatialInertiaRaw(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialInertiaRaw *)  ((iDynTree::SpatialInertia *) x));
+static void *_p_iDynTree__AngularMotionVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularMotionVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::AngularMotionVector3Semantics > *)  ((iDynTree::AngularMotionVector3Semantics *) x));
 }
 static void *_p_iDynTree__ForceVector3SemanticsT_iDynTree__AngularForceVector3Semantics_tTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::GeomVector3Semantics< iDynTree::AngularForceVector3Semantics > *)  ((iDynTree::ForceVector3Semantics< iDynTree::AngularForceVector3Semantics > *) x));
@@ -87824,29 +93948,32 @@ static void *_p_iDynTree__ForceVector3SemanticsT_iDynTree__AngularForceVector3Se
 static void *_p_iDynTree__AngularForceVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::GeomVector3Semantics< iDynTree::AngularForceVector3Semantics > *) (iDynTree::ForceVector3Semantics< iDynTree::AngularForceVector3Semantics > *) ((iDynTree::AngularForceVector3Semantics *) x));
 }
-static void *_p_iDynTree__AngularMotionVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__AngularMotionVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::AngularMotionVector3Semantics > *)  ((iDynTree::AngularMotionVector3Semantics *) x));
+static void *_p_iDynTree__WrenchTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialForceVector > *) (iDynTree::SpatialForceVector *) ((iDynTree::Wrench *) x));
 }
-static void *_p_iDynTree__LinearForceVector3To_p_iDynTree__ForceVector3T_iDynTree__LinearForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::ForceVector3< iDynTree::LinearForceVector3 > *)  ((iDynTree::LinearForceVector3 *) x));
+static void *_p_iDynTree__SpatialForceVectorTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialForceVector > *)  ((iDynTree::SpatialForceVector *) x));
 }
-static void *_p_iDynTree__ForceVector3T_iDynTree__AngularForceVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__AngularForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3< iDynTree::AngularForceVector3 > *)  ((iDynTree::ForceVector3< iDynTree::AngularForceVector3 > *) x));
+static void *_p_iDynTree__SpatialMomentumTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialForceVector > *) (iDynTree::SpatialForceVector *) ((iDynTree::SpatialMomentum *) x));
 }
-static void *_p_iDynTree__AngularForceVector3To_p_iDynTree__GeomVector3T_iDynTree__AngularForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3< iDynTree::AngularForceVector3 > *) (iDynTree::ForceVector3< iDynTree::AngularForceVector3 > *) ((iDynTree::AngularForceVector3 *) x));
+static void *_p_iDynTree__JointPosDoubleArrayTo_p_iDynTree__VectorDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::VectorDynSize *)  ((iDynTree::JointPosDoubleArray *) x));
 }
-static void *_p_iDynTree__AccelerometerSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::AccelerometerSensor *) x));
+static void *_p_iDynTree__JointDOFsDoubleArrayTo_p_iDynTree__VectorDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::VectorDynSize *)  ((iDynTree::JointDOFsDoubleArray *) x));
 }
-static void *_p_iDynTree__ThreeAxisAngularAccelerometerSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::ThreeAxisAngularAccelerometerSensor *) x));
+static void *_p_iDynTree__FrameFreeFloatingJacobianTo_p_iDynTree__MatrixDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MatrixDynSize *)  ((iDynTree::FrameFreeFloatingJacobian *) x));
 }
-static void *_p_iDynTree__ThreeAxisForceTorqueContactSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::ThreeAxisForceTorqueContactSensor *) x));
+static void *_p_iDynTree__MomentumFreeFloatingJacobianTo_p_iDynTree__MatrixDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MatrixDynSize *)  ((iDynTree::MomentumFreeFloatingJacobian *) x));
 }
-static void *_p_iDynTree__GyroscopeSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::GyroscopeSensor *) x));
+static void *_p_iDynTree__FreeFloatingMassMatrixTo_p_iDynTree__MatrixDynSize(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::MatrixDynSize *)  ((iDynTree::FreeFloatingMassMatrix *) x));
+}
+static void *_p_iDynTree__AttitudeQuaternionEKFTo_p_iDynTree__DiscreteExtendedKalmanFilterHelper(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::DiscreteExtendedKalmanFilterHelper *)  ((iDynTree::AttitudeQuaternionEKF *) x));
 }
 static void *_p_iDynTree__TwistTo_p_iDynTree__SpatialMotionVector(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::SpatialMotionVector *)  ((iDynTree::Twist *) x));
@@ -87860,80 +93987,44 @@ static void *_p_iDynTree__WrenchTo_p_iDynTree__SpatialForceVector(void *x, int *
 static void *_p_iDynTree__SpatialMomentumTo_p_iDynTree__SpatialForceVector(void *x, int *SWIGUNUSEDPARM(newmemory)) {
     return (void *)((iDynTree::SpatialForceVector *)  ((iDynTree::SpatialMomentum *) x));
 }
-static void *_p_iDynTree__LinearForceVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::LinearForceVector3Semantics > *) (iDynTree::ForceVector3Semantics< iDynTree::LinearForceVector3Semantics > *) ((iDynTree::LinearForceVector3Semantics *) x));
-}
-static void *_p_iDynTree__ForceVector3SemanticsT_iDynTree__LinearForceVector3Semantics_tTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::LinearForceVector3Semantics > *)  ((iDynTree::ForceVector3Semantics< iDynTree::LinearForceVector3Semantics > *) x));
-}
-static void *_p_iDynTree__LinearMotionVector3SemanticsTo_p_iDynTree__GeomVector3SemanticsT_iDynTree__LinearMotionVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3Semantics< iDynTree::LinearMotionVector3Semantics > *)  ((iDynTree::LinearMotionVector3Semantics *) x));
-}
-static void *_p_iDynTree__RotationalInertiaRawTo_p_iDynTree__MatrixFixSizeT_3_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MatrixFixSize< 3,3 > *)  ((iDynTree::RotationalInertiaRaw *) x));
-}
-static void *_p_iDynTree__RotationRawTo_p_iDynTree__MatrixFixSizeT_3_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MatrixFixSize< 3,3 > *)  ((iDynTree::RotationRaw *) x));
-}
-static void *_p_iDynTree__RotationTo_p_iDynTree__MatrixFixSizeT_3_3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MatrixFixSize< 3,3 > *) (iDynTree::RotationRaw *) ((iDynTree::Rotation *) x));
-}
-static void *_p_iDynTree__MovableJointImplT_1_1_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 1,1 > *) x));
-}
-static void *_p_iDynTree__RevoluteJointTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *) (iDynTree::MovableJointImpl< 1,1 > *) ((iDynTree::RevoluteJoint *) x));
-}
-static void *_p_iDynTree__MovableJointImplT_6_6_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 6,6 > *) x));
-}
-static void *_p_iDynTree__MovableJointImplT_5_5_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 5,5 > *) x));
-}
-static void *_p_iDynTree__PrismaticJointTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *) (iDynTree::MovableJointImpl< 1,1 > *) ((iDynTree::PrismaticJoint *) x));
-}
-static void *_p_iDynTree__MovableJointImplT_4_4_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 4,4 > *) x));
-}
-static void *_p_iDynTree__FixedJointTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::FixedJoint *) x));
+static void *_p_iDynTree__ClassicalAccTo_p_iDynTree__VectorFixSizeT_6_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::VectorFixSize< 6 > *)  ((iDynTree::ClassicalAcc *) x));
 }
-static void *_p_iDynTree__MovableJointImplT_3_3_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 3,3 > *) x));
+static void *_p_iDynTree__JointSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *)  ((iDynTree::JointSensor *) x));
 }
-static void *_p_iDynTree__MovableJointImplT_2_2_tTo_p_iDynTree__IJoint(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::IJoint *)  ((iDynTree::MovableJointImpl< 2,2 > *) x));
+static void *_p_iDynTree__AccelerometerSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::AccelerometerSensor *) x));
 }
-static void *_p_iDynTree__LinearMotionVector3To_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MotionVector3< iDynTree::LinearMotionVector3 > *)  ((iDynTree::LinearMotionVector3 *) x));
+static void *_p_iDynTree__ThreeAxisAngularAccelerometerSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::ThreeAxisAngularAccelerometerSensor *) x));
 }
-static void *_p_iDynTree__AngularForceVector3SemanticsTo_p_iDynTree__ForceVector3SemanticsT_iDynTree__AngularForceVector3Semantics_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::ForceVector3Semantics< iDynTree::AngularForceVector3Semantics > *)  ((iDynTree::AngularForceVector3Semantics *) x));
+static void *_p_iDynTree__ThreeAxisForceTorqueContactSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::ThreeAxisForceTorqueContactSensor *) x));
 }
-static void *_p_iDynTree__ForceVector3T_iDynTree__LinearForceVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3< iDynTree::LinearForceVector3 > *)  ((iDynTree::ForceVector3< iDynTree::LinearForceVector3 > *) x));
+static void *_p_iDynTree__LinkSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *)  ((iDynTree::LinkSensor *) x));
 }
-static void *_p_iDynTree__LinearForceVector3To_p_iDynTree__GeomVector3T_iDynTree__LinearForceVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3< iDynTree::LinearForceVector3 > *) (iDynTree::ForceVector3< iDynTree::LinearForceVector3 > *) ((iDynTree::LinearForceVector3 *) x));
+static void *_p_iDynTree__SixAxisForceTorqueSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *) (iDynTree::JointSensor *) ((iDynTree::SixAxisForceTorqueSensor *) x));
 }
-static void *_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3< iDynTree::LinearMotionVector3 > *)  ((iDynTree::MotionVector3< iDynTree::LinearMotionVector3 > *) x));
+static void *_p_iDynTree__GyroscopeSensorTo_p_iDynTree__Sensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::Sensor *) (iDynTree::LinkSensor *) ((iDynTree::GyroscopeSensor *) x));
 }
-static void *_p_iDynTree__LinearMotionVector3To_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::GeomVector3< iDynTree::LinearMotionVector3 > *) (iDynTree::MotionVector3< iDynTree::LinearMotionVector3 > *) ((iDynTree::LinearMotionVector3 *) x));
+static void *_p_iDynTree__SixAxisForceTorqueSensorTo_p_iDynTree__JointSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::JointSensor *)  ((iDynTree::SixAxisForceTorqueSensor *) x));
 }
-static void *_p_iDynTree__SpatialMotionVectorTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialMotionVector > *)  ((iDynTree::SpatialMotionVector *) x));
+static void *_p_iDynTree__AccelerometerSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::AccelerometerSensor *) x));
 }
-static void *_p_iDynTree__TwistTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialMotionVector > *) (iDynTree::SpatialMotionVector *) ((iDynTree::Twist *) x));
+static void *_p_iDynTree__ThreeAxisAngularAccelerometerSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::ThreeAxisAngularAccelerometerSensor *) x));
 }
-static void *_p_iDynTree__SpatialAccTo_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::SpatialVector< iDynTree::SpatialMotionVector > *) (iDynTree::SpatialMotionVector *) ((iDynTree::SpatialAcc *) x));
+static void *_p_iDynTree__ThreeAxisForceTorqueContactSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::ThreeAxisForceTorqueContactSensor *) x));
 }
-static void *_p_iDynTree__AngularMotionVector3To_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t(void *x, int *SWIGUNUSEDPARM(newmemory)) {
-    return (void *)((iDynTree::MotionVector3< iDynTree::AngularMotionVector3 > *)  ((iDynTree::AngularMotionVector3 *) x));
+static void *_p_iDynTree__GyroscopeSensorTo_p_iDynTree__LinkSensor(void *x, int *SWIGUNUSEDPARM(newmemory)) {
+    return (void *)((iDynTree::LinkSensor *)  ((iDynTree::GyroscopeSensor *) x));
 }
 static swig_type_info _swigt__p_AngularVector3T = {"_p_AngularVector3T", "AngularVector3T *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_AngularVector3Type = {"_p_AngularVector3Type", "AngularVector3Type *", 0, 0, (void*)0, 0};
@@ -87956,9 +94047,11 @@ static swig_type_info _swigt__p_allocator_type = {"_p_allocator_type", "allocato
 static swig_type_info _swigt__p_char = {"_p_char", "char *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_const_iterator = {"_p_const_iterator", "const_iterator *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_const_pointer = {"_p_const_pointer", "const_pointer *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_const_reverse_iterator = {"_p_const_reverse_iterator", "const_reverse_iterator *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_const_typed_iterator = {"_p_const_typed_iterator", "const_typed_iterator *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_difference_type = {"_p_difference_type", "difference_type *", 0, 0, (void*)0, 0};
-static swig_type_info _swigt__p_double = {"_p_double", "double *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_double = {"_p_double", "iDynTree::Span< double,-1 >::element_type *|iDynTree::Span< double,-1 >::pointer|double *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_element_type = {"_p_element_type", "element_type *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_iDynTree__AccelerometerSensor = {"_p_iDynTree__AccelerometerSensor", "iDynTree::AccelerometerSensor *", 0, 0, (void*)"iDynTree.AccelerometerSensor", 0};
 static swig_type_info _swigt__p_iDynTree__AngularForceVector3 = {"_p_iDynTree__AngularForceVector3", "iDynTree::SpatialForceVector::AngularVector3T *|iDynTree::MotionForce_traits< iDynTree::AngularMotionVector3 >::DualSpace *|iDynTree::AngularForceVector3 *|iDynTree::MotionVector3< iDynTree::LinearMotionVector3 >::MotionCrossLinF *|iDynTree::AngMomentum *|iDynTree::SpatialVector< iDynTree::SpatialForceVector >::AngularVector3T *|iDynTree::MotionVector3< iDynTree::AngularMotionVector3 >::MotionCrossAngF *|iDynTree::Torque *", 0, 0, (void*)"iDynTree.AngularForceVector3", 0};
 static swig_type_info _swigt__p_iDynTree__AngularForceVector3Semantics = {"_p_iDynTree__AngularForceVector3Semantics", "iDynTree::DualMotionForceSemanticsT< iDynTree::AngularMotionVector3Semantics >::Type *|iDynTree::AngularForceVector3Semantics *|iDynTree::GeomVector3< iDynTree::AngularForceVector3 >::MotionForceSemanticsT *", 0, 0, (void*)"iDynTree.AngularForceVector3Semantics", 0};
@@ -87966,6 +94059,11 @@ static swig_type_info _swigt__p_iDynTree__AngularMotionVector3 = {"_p_iDynTree__
 static swig_type_info _swigt__p_iDynTree__AngularMotionVector3Semantics = {"_p_iDynTree__AngularMotionVector3Semantics", "iDynTree::DualMotionForceSemanticsT< iDynTree::AngularForceVector3Semantics >::Type *|iDynTree::AngularMotionVector3Semantics *|iDynTree::GeomVector3< iDynTree::AngularMotionVector3 >::MotionForceSemanticsT *", 0, 0, (void*)"iDynTree.AngularMotionVector3Semantics", 0};
 static swig_type_info _swigt__p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers = {"_p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers", "iDynTree::ArticulatedBodyAlgorithmInternalBuffers *", 0, 0, (void*)"iDynTree.ArticulatedBodyAlgorithmInternalBuffers", 0};
 static swig_type_info _swigt__p_iDynTree__ArticulatedBodyInertia = {"_p_iDynTree__ArticulatedBodyInertia", "iDynTree::ArticulatedBodyInertia *", 0, 0, (void*)"iDynTree.ArticulatedBodyInertia", 0};
+static swig_type_info _swigt__p_iDynTree__AttitudeEstimatorState = {"_p_iDynTree__AttitudeEstimatorState", "iDynTree::AttitudeEstimatorState *", 0, 0, (void*)"iDynTree.AttitudeEstimatorState", 0};
+static swig_type_info _swigt__p_iDynTree__AttitudeMahonyFilter = {"_p_iDynTree__AttitudeMahonyFilter", "iDynTree::AttitudeMahonyFilter *", 0, 0, (void*)"iDynTree.AttitudeMahonyFilter", 0};
+static swig_type_info _swigt__p_iDynTree__AttitudeMahonyFilterParameters = {"_p_iDynTree__AttitudeMahonyFilterParameters", "iDynTree::AttitudeMahonyFilterParameters *", 0, 0, (void*)"iDynTree.AttitudeMahonyFilterParameters", 0};
+static swig_type_info _swigt__p_iDynTree__AttitudeQuaternionEKF = {"_p_iDynTree__AttitudeQuaternionEKF", "iDynTree::AttitudeQuaternionEKF *", 0, 0, (void*)"iDynTree.AttitudeQuaternionEKF", 0};
+static swig_type_info _swigt__p_iDynTree__AttitudeQuaternionEKFParameters = {"_p_iDynTree__AttitudeQuaternionEKFParameters", "iDynTree::AttitudeQuaternionEKFParameters *", 0, 0, (void*)"iDynTree.AttitudeQuaternionEKFParameters", 0};
 static swig_type_info _swigt__p_iDynTree__Axis = {"_p_iDynTree__Axis", "iDynTree::Axis *", 0, 0, (void*)"iDynTree.Axis", 0};
 static swig_type_info _swigt__p_iDynTree__BerdyDynamicVariable = {"_p_iDynTree__BerdyDynamicVariable", "std::vector< iDynTree::BerdyDynamicVariable >::value_type *|iDynTree::BerdyDynamicVariable *", 0, 0, (void*)"iDynTree.BerdyDynamicVariable", 0};
 static swig_type_info _swigt__p_iDynTree__BerdyHelper = {"_p_iDynTree__BerdyHelper", "iDynTree::BerdyHelper *", 0, 0, (void*)"iDynTree.BerdyHelper", 0};
@@ -87980,6 +94078,7 @@ static swig_type_info _swigt__p_iDynTree__Cylinder = {"_p_iDynTree__Cylinder", "
 static swig_type_info _swigt__p_iDynTree__DOFSpatialForceArray = {"_p_iDynTree__DOFSpatialForceArray", "iDynTree::DOFSpatialForceArray *", 0, 0, (void*)"iDynTree.DOFSpatialForceArray", 0};
 static swig_type_info _swigt__p_iDynTree__DOFSpatialMotionArray = {"_p_iDynTree__DOFSpatialMotionArray", "iDynTree::DOFSpatialMotionArray *", 0, 0, (void*)"iDynTree.DOFSpatialMotionArray", 0};
 static swig_type_info _swigt__p_iDynTree__Direction = {"_p_iDynTree__Direction", "iDynTree::Direction *", 0, 0, (void*)"iDynTree.Direction", 0};
+static swig_type_info _swigt__p_iDynTree__DiscreteExtendedKalmanFilterHelper = {"_p_iDynTree__DiscreteExtendedKalmanFilterHelper", "iDynTree::DiscreteExtendedKalmanFilterHelper *", 0, 0, (void*)"iDynTree.DiscreteExtendedKalmanFilterHelper", 0};
 static swig_type_info _swigt__p_iDynTree__Dummy = {"_p_iDynTree__Dummy", "iDynTree::Dummy *", 0, 0, (void*)"iDynTree.Dummy", 0};
 static swig_type_info _swigt__p_iDynTree__ExtWrenchesAndJointTorquesEstimator = {"_p_iDynTree__ExtWrenchesAndJointTorquesEstimator", "iDynTree::ExtWrenchesAndJointTorquesEstimator *", 0, 0, (void*)"iDynTree.ExtWrenchesAndJointTorquesEstimator", 0};
 static swig_type_info _swigt__p_iDynTree__ExternalMesh = {"_p_iDynTree__ExternalMesh", "iDynTree::ExternalMesh *", 0, 0, (void*)"iDynTree.ExternalMesh", 0};
@@ -88004,12 +94103,14 @@ static swig_type_info _swigt__p_iDynTree__GeomVector3T_iDynTree__LinearForceVect
 static swig_type_info _swigt__p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t = {"_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t", "iDynTree::GeomVector3< iDynTree::LinearMotionVector3 > *|iDynTree::GeomVector3< iDynTree::LinearMotionVector3 >::MotionForceTbase *", 0, 0, (void*)"iDynTree.GeomVector3__LinearMotionVector3", 0};
 static swig_type_info _swigt__p_iDynTree__GyroscopeSensor = {"_p_iDynTree__GyroscopeSensor", "iDynTree::GyroscopeSensor *", 0, 0, (void*)"iDynTree.GyroscopeSensor", 0};
 static swig_type_info _swigt__p_iDynTree__HighLevel__DynamicsComputations = {"_p_iDynTree__HighLevel__DynamicsComputations", "iDynTree::HighLevel::DynamicsComputations *", 0, 0, (void*)"iDynTree.DynamicsComputations", 0};
+static swig_type_info _swigt__p_iDynTree__IAttitudeEstimator = {"_p_iDynTree__IAttitudeEstimator", "iDynTree::IAttitudeEstimator *", 0, 0, (void*)"iDynTree.IAttitudeEstimator", 0};
 static swig_type_info _swigt__p_iDynTree__ICamera = {"_p_iDynTree__ICamera", "iDynTree::ICamera *", 0, 0, (void*)"iDynTree.ICamera", 0};
 static swig_type_info _swigt__p_iDynTree__IEnvironment = {"_p_iDynTree__IEnvironment", "iDynTree::IEnvironment *", 0, 0, (void*)"iDynTree.IEnvironment", 0};
 static swig_type_info _swigt__p_iDynTree__IJetsVisualization = {"_p_iDynTree__IJetsVisualization", "iDynTree::IJetsVisualization *", 0, 0, (void*)"iDynTree.IJetsVisualization", 0};
 static swig_type_info _swigt__p_iDynTree__IJoint = {"_p_iDynTree__IJoint", "iDynTree::IJointPtr|iDynTree::IJoint *|iDynTree::IJointConstPtr", 0, 0, (void*)"iDynTree.IJoint", 0};
 static swig_type_info _swigt__p_iDynTree__ILight = {"_p_iDynTree__ILight", "iDynTree::ILight *", 0, 0, (void*)"iDynTree.ILight", 0};
 static swig_type_info _swigt__p_iDynTree__IModelVisualization = {"_p_iDynTree__IModelVisualization", "iDynTree::IModelVisualization *", 0, 0, (void*)"iDynTree.IModelVisualization", 0};
+static swig_type_info _swigt__p_iDynTree__IVectorsVisualization = {"_p_iDynTree__IVectorsVisualization", "iDynTree::IVectorsVisualization *", 0, 0, (void*)"iDynTree.IVectorsVisualization", 0};
 static swig_type_info _swigt__p_iDynTree__IndexRange = {"_p_iDynTree__IndexRange", "iDynTree::IndexRange *", 0, 0, (void*)"iDynTree.IndexRange", 0};
 static swig_type_info _swigt__p_iDynTree__JointDOFsDoubleArray = {"_p_iDynTree__JointDOFsDoubleArray", "iDynTree::JointDOFsDoubleArray *", 0, 0, (void*)"iDynTree.JointDOFsDoubleArray", 0};
 static swig_type_info _swigt__p_iDynTree__JointPosDoubleArray = {"_p_iDynTree__JointPosDoubleArray", "iDynTree::JointPosDoubleArray *", 0, 0, (void*)"iDynTree.JointPosDoubleArray", 0};
@@ -88073,6 +94174,7 @@ static swig_type_info _swigt__p_iDynTree__SensorsMeasurements = {"_p_iDynTree__S
 static swig_type_info _swigt__p_iDynTree__SimpleLeggedOdometry = {"_p_iDynTree__SimpleLeggedOdometry", "iDynTree::SimpleLeggedOdometry *", 0, 0, (void*)"iDynTree.SimpleLeggedOdometry", 0};
 static swig_type_info _swigt__p_iDynTree__SixAxisForceTorqueSensor = {"_p_iDynTree__SixAxisForceTorqueSensor", "iDynTree::SixAxisForceTorqueSensor *", 0, 0, (void*)"iDynTree.SixAxisForceTorqueSensor", 0};
 static swig_type_info _swigt__p_iDynTree__SolidShape = {"_p_iDynTree__SolidShape", "iDynTree::SolidShape *", 0, 0, (void*)"iDynTree.SolidShape", 0};
+static swig_type_info _swigt__p_iDynTree__SpanT_double__1_t = {"_p_iDynTree__SpanT_double__1_t", "iDynTree::Span< iDynTree::Span< double,-1 >::element_type,-1 > *|iDynTree::Span< double,-1 > *", 0, 0, (void*)"iDynTree.DynamicSpan", 0};
 static swig_type_info _swigt__p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t = {"_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t", "iDynTree::SparseMatrix< iDynTree::ColumnMajor > *", 0, 0, (void*)"iDynTree.SparseMatrixColMajor", 0};
 static swig_type_info _swigt__p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t = {"_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t", "iDynTree::SparseMatrix< iDynTree::RowMajor > *", 0, 0, (void*)"iDynTree.SparseMatrixRowMajor", 0};
 static swig_type_info _swigt__p_iDynTree__SpatialAcc = {"_p_iDynTree__SpatialAcc", "iDynTree::SpatialAcc *", 0, 0, (void*)"iDynTree.SpatialAcc", 0};
@@ -88101,20 +94203,27 @@ static swig_type_info _swigt__p_iDynTree__VectorDynSize = {"_p_iDynTree__VectorD
 static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_10_t = {"_p_iDynTree__VectorFixSizeT_10_t", "iDynTree::VectorFixSize< 10 > *|iDynTree::Vector10 *", 0, 0, (void*)"iDynTree.Vector10", 0};
 static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_16_t = {"_p_iDynTree__VectorFixSizeT_16_t", "iDynTree::Vector16 *|iDynTree::VectorFixSize< 16 > *", 0, 0, (void*)"iDynTree.Vector16", 0};
 static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_2_t = {"_p_iDynTree__VectorFixSizeT_2_t", "iDynTree::Vector2 *|iDynTree::VectorFixSize< 2 > *", 0, 0, (void*)0, 0};
-static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_3_t = {"_p_iDynTree__VectorFixSizeT_3_t", "iDynTree::Vector3 *|iDynTree::VectorFixSize< 3 > *", 0, 0, (void*)"iDynTree.Vector3", 0};
-static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_4_t = {"_p_iDynTree__VectorFixSizeT_4_t", "iDynTree::VectorFixSize< 4 > *|iDynTree::Vector4 *", 0, 0, (void*)"iDynTree.Vector4", 0};
+static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_3_t = {"_p_iDynTree__VectorFixSizeT_3_t", "iDynTree::MagnetometerMeasurements *|iDynTree::GyroscopeMeasurements *|iDynTree::LinearAccelerometerMeasurements *|iDynTree::Vector3 *|iDynTree::RPY *|iDynTree::VectorFixSize< 3 > *", 0, 0, (void*)"iDynTree.Vector3", 0};
+static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_4_t = {"_p_iDynTree__VectorFixSizeT_4_t", "iDynTree::VectorFixSize< 4 > *|iDynTree::Vector4 *|iDynTree::UnitQuaternion *", 0, 0, (void*)"iDynTree.Vector4", 0};
 static swig_type_info _swigt__p_iDynTree__VectorFixSizeT_6_t = {"_p_iDynTree__VectorFixSizeT_6_t", "iDynTree::Vector6 *|iDynTree::VectorFixSize< 6 > *", 0, 0, (void*)"iDynTree.Vector6", 0};
 static swig_type_info _swigt__p_iDynTree__Visualizer = {"_p_iDynTree__Visualizer", "iDynTree::Visualizer *", 0, 0, (void*)"iDynTree.Visualizer", 0};
 static swig_type_info _swigt__p_iDynTree__VisualizerOptions = {"_p_iDynTree__VisualizerOptions", "iDynTree::VisualizerOptions *", 0, 0, (void*)"iDynTree.VisualizerOptions", 0};
 static swig_type_info _swigt__p_iDynTree__Wrench = {"_p_iDynTree__Wrench", "iDynTree::Wrench *", 0, 0, (void*)"iDynTree.Wrench", 0};
+static swig_type_info _swigt__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t = {"_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t", "iDynTree::Span< double,-1 >::iterator *|iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,false > *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t = {"_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t", "iDynTree::Span< double,-1 >::const_iterator *|iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,true > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_iDynTree__estimateExternalWrenchesBuffers = {"_p_iDynTree__estimateExternalWrenchesBuffers", "iDynTree::estimateExternalWrenchesBuffers *", 0, 0, (void*)"iDynTree.estimateExternalWrenchesBuffers", 0};
+static swig_type_info _swigt__p_index_type = {"_p_index_type", "index_type *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_int = {"_p_int", "int *|iDynTree::LinkIndex *|iDynTree::FrameIndex *|iDynTree::TraversalIndex *|iDynTree::JointIndex *|iDynTree::DOFIndex *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_iterator = {"_p_iterator", "iterator *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_pointer = {"_p_pointer", "pointer *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_reference = {"_p_reference", "reference *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_reverse_iterator = {"_p_reverse_iterator", "reverse_iterator *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_size_type = {"_p_size_type", "size_type *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__allocatorT_iDynTree__BerdyDynamicVariable_t = {"_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t", "std::vector< iDynTree::BerdyDynamicVariable >::allocator_type *|std::allocator< iDynTree::BerdyDynamicVariable > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__allocatorT_iDynTree__BerdySensor_t = {"_p_std__allocatorT_iDynTree__BerdySensor_t", "std::allocator< iDynTree::BerdySensor > *|std::vector< iDynTree::BerdySensor >::allocator_type *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__allocatorT_std__string_t = {"_p_std__allocatorT_std__string_t", "std::vector< std::string >::allocator_type *|std::allocator< std::string > *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t = {"_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t", "iDynTree::Span< double,-1 >::reverse_iterator *|std::reverse_iterator< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,false > > *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t = {"_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t", "std::reverse_iterator< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,true > > *|iDynTree::Span< double,-1 >::const_reverse_iterator *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t = {"_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t", "std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > *|std::vector< iDynTree::BerdyDynamicVariable > *", 0, 0, (void*)"iDynTree.BerdyDynamicVariables", 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t = {"_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t", "std::vector< iDynTree::BerdySensor > *|std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > *", 0, 0, (void*)"iDynTree.BerdySensors", 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t = {"_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t", "std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *", 0, 0, (void*)0, 0};
@@ -88152,9 +94261,11 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_char,
   &_swigt__p_const_iterator,
   &_swigt__p_const_pointer,
+  &_swigt__p_const_reverse_iterator,
   &_swigt__p_const_typed_iterator,
   &_swigt__p_difference_type,
   &_swigt__p_double,
+  &_swigt__p_element_type,
   &_swigt__p_iDynTree__AccelerometerSensor,
   &_swigt__p_iDynTree__AngularForceVector3,
   &_swigt__p_iDynTree__AngularForceVector3Semantics,
@@ -88162,6 +94273,11 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__AngularMotionVector3Semantics,
   &_swigt__p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers,
   &_swigt__p_iDynTree__ArticulatedBodyInertia,
+  &_swigt__p_iDynTree__AttitudeEstimatorState,
+  &_swigt__p_iDynTree__AttitudeMahonyFilter,
+  &_swigt__p_iDynTree__AttitudeMahonyFilterParameters,
+  &_swigt__p_iDynTree__AttitudeQuaternionEKF,
+  &_swigt__p_iDynTree__AttitudeQuaternionEKFParameters,
   &_swigt__p_iDynTree__Axis,
   &_swigt__p_iDynTree__BerdyDynamicVariable,
   &_swigt__p_iDynTree__BerdyHelper,
@@ -88176,6 +94292,7 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__DOFSpatialForceArray,
   &_swigt__p_iDynTree__DOFSpatialMotionArray,
   &_swigt__p_iDynTree__Direction,
+  &_swigt__p_iDynTree__DiscreteExtendedKalmanFilterHelper,
   &_swigt__p_iDynTree__Dummy,
   &_swigt__p_iDynTree__ExtWrenchesAndJointTorquesEstimator,
   &_swigt__p_iDynTree__ExternalMesh,
@@ -88200,12 +94317,14 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t,
   &_swigt__p_iDynTree__GyroscopeSensor,
   &_swigt__p_iDynTree__HighLevel__DynamicsComputations,
+  &_swigt__p_iDynTree__IAttitudeEstimator,
   &_swigt__p_iDynTree__ICamera,
   &_swigt__p_iDynTree__IEnvironment,
   &_swigt__p_iDynTree__IJetsVisualization,
   &_swigt__p_iDynTree__IJoint,
   &_swigt__p_iDynTree__ILight,
   &_swigt__p_iDynTree__IModelVisualization,
+  &_swigt__p_iDynTree__IVectorsVisualization,
   &_swigt__p_iDynTree__IndexRange,
   &_swigt__p_iDynTree__JointDOFsDoubleArray,
   &_swigt__p_iDynTree__JointPosDoubleArray,
@@ -88269,6 +94388,7 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__SimpleLeggedOdometry,
   &_swigt__p_iDynTree__SixAxisForceTorqueSensor,
   &_swigt__p_iDynTree__SolidShape,
+  &_swigt__p_iDynTree__SpanT_double__1_t,
   &_swigt__p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,
   &_swigt__p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t,
   &_swigt__p_iDynTree__SpatialAcc,
@@ -88303,14 +94423,21 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__Visualizer,
   &_swigt__p_iDynTree__VisualizerOptions,
   &_swigt__p_iDynTree__Wrench,
+  &_swigt__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t,
+  &_swigt__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t,
   &_swigt__p_iDynTree__estimateExternalWrenchesBuffers,
+  &_swigt__p_index_type,
   &_swigt__p_int,
   &_swigt__p_iterator,
   &_swigt__p_pointer,
+  &_swigt__p_reference,
+  &_swigt__p_reverse_iterator,
   &_swigt__p_size_type,
   &_swigt__p_std__allocatorT_iDynTree__BerdyDynamicVariable_t,
   &_swigt__p_std__allocatorT_iDynTree__BerdySensor_t,
   &_swigt__p_std__allocatorT_std__string_t,
+  &_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t,
+  &_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t,
   &_swigt__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t,
   &_swigt__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t,
   &_swigt__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t,
@@ -88348,9 +94475,11 @@ static swig_cast_info _swigc__p_allocator_type[] = {  {&_swigt__p_allocator_type
 static swig_cast_info _swigc__p_char[] = {  {&_swigt__p_char, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_const_iterator[] = {  {&_swigt__p_const_iterator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_const_pointer[] = {  {&_swigt__p_const_pointer, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_const_reverse_iterator[] = {  {&_swigt__p_const_reverse_iterator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_const_typed_iterator[] = {  {&_swigt__p_const_typed_iterator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_difference_type[] = {  {&_swigt__p_difference_type, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_double[] = {  {&_swigt__p_double, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_element_type[] = {  {&_swigt__p_element_type, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__AccelerometerSensor[] = {  {&_swigt__p_iDynTree__AccelerometerSensor, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__AngularForceVector3[] = {  {&_swigt__p_iDynTree__AngularForceVector3, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__AngularForceVector3Semantics[] = {  {&_swigt__p_iDynTree__AngularForceVector3Semantics, 0, 0, 0},{0, 0, 0, 0}};
@@ -88358,6 +94487,11 @@ static swig_cast_info _swigc__p_iDynTree__AngularMotionVector3[] = {  {&_swigt__
 static swig_cast_info _swigc__p_iDynTree__AngularMotionVector3Semantics[] = {  {&_swigt__p_iDynTree__AngularMotionVector3Semantics, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers[] = {  {&_swigt__p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ArticulatedBodyInertia[] = {  {&_swigt__p_iDynTree__ArticulatedBodyInertia, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__AttitudeEstimatorState[] = {  {&_swigt__p_iDynTree__AttitudeEstimatorState, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__AttitudeMahonyFilter[] = {  {&_swigt__p_iDynTree__AttitudeMahonyFilter, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__AttitudeMahonyFilterParameters[] = {  {&_swigt__p_iDynTree__AttitudeMahonyFilterParameters, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__AttitudeQuaternionEKF[] = {  {&_swigt__p_iDynTree__AttitudeQuaternionEKF, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__AttitudeQuaternionEKFParameters[] = {  {&_swigt__p_iDynTree__AttitudeQuaternionEKFParameters, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__Axis[] = {  {&_swigt__p_iDynTree__Axis, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__BerdyDynamicVariable[] = {  {&_swigt__p_iDynTree__BerdyDynamicVariable, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__BerdyHelper[] = {  {&_swigt__p_iDynTree__BerdyHelper, 0, 0, 0},{0, 0, 0, 0}};
@@ -88372,6 +94506,7 @@ static swig_cast_info _swigc__p_iDynTree__Cylinder[] = {  {&_swigt__p_iDynTree__
 static swig_cast_info _swigc__p_iDynTree__DOFSpatialForceArray[] = {  {&_swigt__p_iDynTree__DOFSpatialForceArray, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__DOFSpatialMotionArray[] = {  {&_swigt__p_iDynTree__DOFSpatialMotionArray, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__Direction[] = {  {&_swigt__p_iDynTree__Direction, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__DiscreteExtendedKalmanFilterHelper[] = {  {&_swigt__p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0, 0, 0},  {&_swigt__p_iDynTree__AttitudeQuaternionEKF, _p_iDynTree__AttitudeQuaternionEKFTo_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__Dummy[] = {  {&_swigt__p_iDynTree__Dummy, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ExtWrenchesAndJointTorquesEstimator[] = {  {&_swigt__p_iDynTree__ExtWrenchesAndJointTorquesEstimator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ExternalMesh[] = {  {&_swigt__p_iDynTree__ExternalMesh, 0, 0, 0},{0, 0, 0, 0}};
@@ -88396,12 +94531,14 @@ static swig_cast_info _swigc__p_iDynTree__GeomVector3T_iDynTree__LinearForceVect
 static swig_cast_info _swigc__p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t[] = {  {&_swigt__p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t, 0, 0, 0},  {&_swigt__p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t, _p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_tTo_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t, 0, 0},  {&_swigt__p_iDynTree__LinearMotionVector3, _p_iDynTree__LinearMotionVector3To_p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__GyroscopeSensor[] = {  {&_swigt__p_iDynTree__GyroscopeSensor, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__HighLevel__DynamicsComputations[] = {  {&_swigt__p_iDynTree__HighLevel__DynamicsComputations, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__IAttitudeEstimator[] = {  {&_swigt__p_iDynTree__AttitudeMahonyFilter, _p_iDynTree__AttitudeMahonyFilterTo_p_iDynTree__IAttitudeEstimator, 0, 0},  {&_swigt__p_iDynTree__AttitudeQuaternionEKF, _p_iDynTree__AttitudeQuaternionEKFTo_p_iDynTree__IAttitudeEstimator, 0, 0},  {&_swigt__p_iDynTree__IAttitudeEstimator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ICamera[] = {  {&_swigt__p_iDynTree__ICamera, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__IEnvironment[] = {  {&_swigt__p_iDynTree__IEnvironment, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__IJetsVisualization[] = {  {&_swigt__p_iDynTree__IJetsVisualization, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__IJoint[] = {  {&_swigt__p_iDynTree__MovableJointImplT_3_3_t, _p_iDynTree__MovableJointImplT_3_3_tTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__RevoluteJoint, _p_iDynTree__RevoluteJointTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__MovableJointImplT_2_2_t, _p_iDynTree__MovableJointImplT_2_2_tTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__IJoint, 0, 0, 0},  {&_swigt__p_iDynTree__MovableJointImplT_1_1_t, _p_iDynTree__MovableJointImplT_1_1_tTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__PrismaticJoint, _p_iDynTree__PrismaticJointTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__MovableJointImplT_6_6_t, _p_iDynTree__MovableJointImplT_6_6_tTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__MovableJointImplT_5_5_t, _p_iDynTree__MovableJointImplT_5_5_tTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__FixedJoint, _p_iDynTree__FixedJointTo_p_iDynTree__IJoint, 0, 0},  {&_swigt__p_iDynTree__MovableJointImplT_4_4_t, _p_iDynTree__MovableJointImplT_4_4_tTo_p_iDynTree__IJoint, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ILight[] = {  {&_swigt__p_iDynTree__ILight, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__IModelVisualization[] = {  {&_swigt__p_iDynTree__IModelVisualization, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__IVectorsVisualization[] = {  {&_swigt__p_iDynTree__IVectorsVisualization, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__IndexRange[] = {  {&_swigt__p_iDynTree__IndexRange, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__JointDOFsDoubleArray[] = {  {&_swigt__p_iDynTree__JointDOFsDoubleArray, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__JointPosDoubleArray[] = {  {&_swigt__p_iDynTree__JointPosDoubleArray, 0, 0, 0},{0, 0, 0, 0}};
@@ -88465,6 +94602,7 @@ static swig_cast_info _swigc__p_iDynTree__SensorsMeasurements[] = {  {&_swigt__p
 static swig_cast_info _swigc__p_iDynTree__SimpleLeggedOdometry[] = {  {&_swigt__p_iDynTree__SimpleLeggedOdometry, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__SixAxisForceTorqueSensor[] = {  {&_swigt__p_iDynTree__SixAxisForceTorqueSensor, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__SolidShape[] = {  {&_swigt__p_iDynTree__SolidShape, 0, 0, 0},  {&_swigt__p_iDynTree__ExternalMesh, _p_iDynTree__ExternalMeshTo_p_iDynTree__SolidShape, 0, 0},  {&_swigt__p_iDynTree__Box, _p_iDynTree__BoxTo_p_iDynTree__SolidShape, 0, 0},  {&_swigt__p_iDynTree__Cylinder, _p_iDynTree__CylinderTo_p_iDynTree__SolidShape, 0, 0},  {&_swigt__p_iDynTree__Sphere, _p_iDynTree__SphereTo_p_iDynTree__SolidShape, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__SpanT_double__1_t[] = {  {&_swigt__p_iDynTree__SpanT_double__1_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t[] = {  {&_swigt__p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t[] = {  {&_swigt__p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__SpatialAcc[] = {  {&_swigt__p_iDynTree__SpatialAcc, 0, 0, 0},{0, 0, 0, 0}};
@@ -88499,14 +94637,21 @@ static swig_cast_info _swigc__p_iDynTree__VectorFixSizeT_6_t[] = {  {&_swigt__p_
 static swig_cast_info _swigc__p_iDynTree__Visualizer[] = {  {&_swigt__p_iDynTree__Visualizer, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__VisualizerOptions[] = {  {&_swigt__p_iDynTree__VisualizerOptions, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__Wrench[] = {  {&_swigt__p_iDynTree__Wrench, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t[] = {  {&_swigt__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t[] = {  {&_swigt__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__estimateExternalWrenchesBuffers[] = {  {&_swigt__p_iDynTree__estimateExternalWrenchesBuffers, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_index_type[] = {  {&_swigt__p_index_type, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_int[] = {  {&_swigt__p_int, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iterator[] = {  {&_swigt__p_iterator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_pointer[] = {  {&_swigt__p_pointer, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_reference[] = {  {&_swigt__p_reference, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_reverse_iterator[] = {  {&_swigt__p_reverse_iterator, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_size_type[] = {  {&_swigt__p_size_type, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__allocatorT_iDynTree__BerdyDynamicVariable_t[] = {  {&_swigt__p_std__allocatorT_iDynTree__BerdyDynamicVariable_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__allocatorT_iDynTree__BerdySensor_t[] = {  {&_swigt__p_std__allocatorT_iDynTree__BerdySensor_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__allocatorT_std__string_t[] = {  {&_swigt__p_std__allocatorT_std__string_t, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t[] = {  {&_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t[] = {  {&_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0, 0, 0},{0, 0, 0, 0}};
@@ -88544,9 +94689,11 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_char,
   _swigc__p_const_iterator,
   _swigc__p_const_pointer,
+  _swigc__p_const_reverse_iterator,
   _swigc__p_const_typed_iterator,
   _swigc__p_difference_type,
   _swigc__p_double,
+  _swigc__p_element_type,
   _swigc__p_iDynTree__AccelerometerSensor,
   _swigc__p_iDynTree__AngularForceVector3,
   _swigc__p_iDynTree__AngularForceVector3Semantics,
@@ -88554,6 +94701,11 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__AngularMotionVector3Semantics,
   _swigc__p_iDynTree__ArticulatedBodyAlgorithmInternalBuffers,
   _swigc__p_iDynTree__ArticulatedBodyInertia,
+  _swigc__p_iDynTree__AttitudeEstimatorState,
+  _swigc__p_iDynTree__AttitudeMahonyFilter,
+  _swigc__p_iDynTree__AttitudeMahonyFilterParameters,
+  _swigc__p_iDynTree__AttitudeQuaternionEKF,
+  _swigc__p_iDynTree__AttitudeQuaternionEKFParameters,
   _swigc__p_iDynTree__Axis,
   _swigc__p_iDynTree__BerdyDynamicVariable,
   _swigc__p_iDynTree__BerdyHelper,
@@ -88568,6 +94720,7 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__DOFSpatialForceArray,
   _swigc__p_iDynTree__DOFSpatialMotionArray,
   _swigc__p_iDynTree__Direction,
+  _swigc__p_iDynTree__DiscreteExtendedKalmanFilterHelper,
   _swigc__p_iDynTree__Dummy,
   _swigc__p_iDynTree__ExtWrenchesAndJointTorquesEstimator,
   _swigc__p_iDynTree__ExternalMesh,
@@ -88592,12 +94745,14 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__GeomVector3T_iDynTree__LinearMotionVector3_t,
   _swigc__p_iDynTree__GyroscopeSensor,
   _swigc__p_iDynTree__HighLevel__DynamicsComputations,
+  _swigc__p_iDynTree__IAttitudeEstimator,
   _swigc__p_iDynTree__ICamera,
   _swigc__p_iDynTree__IEnvironment,
   _swigc__p_iDynTree__IJetsVisualization,
   _swigc__p_iDynTree__IJoint,
   _swigc__p_iDynTree__ILight,
   _swigc__p_iDynTree__IModelVisualization,
+  _swigc__p_iDynTree__IVectorsVisualization,
   _swigc__p_iDynTree__IndexRange,
   _swigc__p_iDynTree__JointDOFsDoubleArray,
   _swigc__p_iDynTree__JointPosDoubleArray,
@@ -88661,6 +94816,7 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__SimpleLeggedOdometry,
   _swigc__p_iDynTree__SixAxisForceTorqueSensor,
   _swigc__p_iDynTree__SolidShape,
+  _swigc__p_iDynTree__SpanT_double__1_t,
   _swigc__p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t,
   _swigc__p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t,
   _swigc__p_iDynTree__SpatialAcc,
@@ -88695,14 +94851,21 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__Visualizer,
   _swigc__p_iDynTree__VisualizerOptions,
   _swigc__p_iDynTree__Wrench,
+  _swigc__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t,
+  _swigc__p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t,
   _swigc__p_iDynTree__estimateExternalWrenchesBuffers,
+  _swigc__p_index_type,
   _swigc__p_int,
   _swigc__p_iterator,
   _swigc__p_pointer,
+  _swigc__p_reference,
+  _swigc__p_reverse_iterator,
   _swigc__p_size_type,
   _swigc__p_std__allocatorT_iDynTree__BerdyDynamicVariable_t,
   _swigc__p_std__allocatorT_iDynTree__BerdySensor_t,
   _swigc__p_std__allocatorT_std__string_t,
+  _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t,
+  _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t,
   _swigc__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t,
   _swigc__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t,
   _swigc__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t,
@@ -89934,1014 +96097,1171 @@ SWIGINTERN const char* SwigFunctionName(int fcn_id) {
   case 813: return "TransformDerivative_mtimes";
   case 814: return "TransformDerivative_derivativeOfInverse";
   case 815: return "TransformDerivative_transform";
-  case 816: return "LINK_INVALID_INDEX_get";
-  case 817: return "LINK_INVALID_INDEX_set";
-  case 818: return "LINK_INVALID_NAME_get";
-  case 819: return "LINK_INVALID_NAME_set";
-  case 820: return "JOINT_INVALID_INDEX_get";
-  case 821: return "JOINT_INVALID_INDEX_set";
-  case 822: return "JOINT_INVALID_NAME_get";
-  case 823: return "JOINT_INVALID_NAME_set";
-  case 824: return "DOF_INVALID_INDEX_get";
-  case 825: return "DOF_INVALID_INDEX_set";
-  case 826: return "DOF_INVALID_NAME_get";
-  case 827: return "DOF_INVALID_NAME_set";
-  case 828: return "FRAME_INVALID_INDEX_get";
-  case 829: return "FRAME_INVALID_INDEX_set";
-  case 830: return "FRAME_INVALID_NAME_get";
-  case 831: return "FRAME_INVALID_NAME_set";
-  case 832: return "TRAVERSAL_INVALID_INDEX_get";
-  case 833: return "TRAVERSAL_INVALID_INDEX_set";
-  case 834: return "new_LinkPositions";
-  case 835: return "LinkPositions_resize";
-  case 836: return "LinkPositions_isConsistent";
-  case 837: return "LinkPositions_getNrOfLinks";
-  case 838: return "LinkPositions_paren";
-  case 839: return "LinkPositions_toString";
-  case 840: return "delete_LinkPositions";
-  case 841: return "new_LinkWrenches";
-  case 842: return "LinkWrenches_resize";
-  case 843: return "LinkWrenches_isConsistent";
-  case 844: return "LinkWrenches_getNrOfLinks";
-  case 845: return "LinkWrenches_paren";
-  case 846: return "LinkWrenches_toString";
-  case 847: return "LinkWrenches_zero";
-  case 848: return "delete_LinkWrenches";
-  case 849: return "new_LinkInertias";
-  case 850: return "LinkInertias_resize";
-  case 851: return "LinkInertias_isConsistent";
-  case 852: return "LinkInertias_paren";
-  case 853: return "delete_LinkInertias";
-  case 854: return "new_LinkArticulatedBodyInertias";
-  case 855: return "LinkArticulatedBodyInertias_resize";
-  case 856: return "LinkArticulatedBodyInertias_isConsistent";
-  case 857: return "LinkArticulatedBodyInertias_paren";
-  case 858: return "delete_LinkArticulatedBodyInertias";
-  case 859: return "new_LinkVelArray";
-  case 860: return "LinkVelArray_resize";
-  case 861: return "LinkVelArray_isConsistent";
-  case 862: return "LinkVelArray_getNrOfLinks";
-  case 863: return "LinkVelArray_paren";
-  case 864: return "LinkVelArray_toString";
-  case 865: return "delete_LinkVelArray";
-  case 866: return "new_LinkAccArray";
-  case 867: return "LinkAccArray_resize";
-  case 868: return "LinkAccArray_isConsistent";
-  case 869: return "LinkAccArray_paren";
-  case 870: return "LinkAccArray_getNrOfLinks";
-  case 871: return "LinkAccArray_toString";
-  case 872: return "delete_LinkAccArray";
-  case 873: return "new_Link";
-  case 874: return "Link_inertia";
-  case 875: return "Link_setInertia";
-  case 876: return "Link_getInertia";
-  case 877: return "Link_setIndex";
-  case 878: return "Link_getIndex";
-  case 879: return "delete_Link";
-  case 880: return "delete_IJoint";
-  case 881: return "IJoint_clone";
-  case 882: return "IJoint_getNrOfPosCoords";
-  case 883: return "IJoint_getNrOfDOFs";
-  case 884: return "IJoint_setAttachedLinks";
-  case 885: return "IJoint_setRestTransform";
-  case 886: return "IJoint_getFirstAttachedLink";
-  case 887: return "IJoint_getSecondAttachedLink";
-  case 888: return "IJoint_getRestTransform";
-  case 889: return "IJoint_getTransform";
-  case 890: return "IJoint_getTransformDerivative";
-  case 891: return "IJoint_getMotionSubspaceVector";
-  case 892: return "IJoint_computeChildPosVelAcc";
-  case 893: return "IJoint_computeChildVelAcc";
-  case 894: return "IJoint_computeChildVel";
-  case 895: return "IJoint_computeChildAcc";
-  case 896: return "IJoint_computeChildBiasAcc";
-  case 897: return "IJoint_computeJointTorque";
-  case 898: return "IJoint_setIndex";
-  case 899: return "IJoint_getIndex";
-  case 900: return "IJoint_setPosCoordsOffset";
-  case 901: return "IJoint_getPosCoordsOffset";
-  case 902: return "IJoint_setDOFsOffset";
-  case 903: return "IJoint_getDOFsOffset";
-  case 904: return "IJoint_hasPosLimits";
-  case 905: return "IJoint_enablePosLimits";
-  case 906: return "IJoint_getPosLimits";
-  case 907: return "IJoint_getMinPosLimit";
-  case 908: return "IJoint_getMaxPosLimit";
-  case 909: return "IJoint_setPosLimits";
-  case 910: return "IJoint_isRevoluteJoint";
-  case 911: return "IJoint_isFixedJoint";
-  case 912: return "IJoint_asRevoluteJoint";
-  case 913: return "IJoint_asFixedJoint";
-  case 914: return "new_FixedJoint";
-  case 915: return "delete_FixedJoint";
-  case 916: return "FixedJoint_clone";
-  case 917: return "FixedJoint_getNrOfPosCoords";
-  case 918: return "FixedJoint_getNrOfDOFs";
-  case 919: return "FixedJoint_setAttachedLinks";
-  case 920: return "FixedJoint_setRestTransform";
-  case 921: return "FixedJoint_getFirstAttachedLink";
-  case 922: return "FixedJoint_getSecondAttachedLink";
-  case 923: return "FixedJoint_getRestTransform";
-  case 924: return "FixedJoint_getTransform";
-  case 925: return "FixedJoint_getTransformDerivative";
-  case 926: return "FixedJoint_getMotionSubspaceVector";
-  case 927: return "FixedJoint_computeChildPosVelAcc";
-  case 928: return "FixedJoint_computeChildVelAcc";
-  case 929: return "FixedJoint_computeChildVel";
-  case 930: return "FixedJoint_computeChildAcc";
-  case 931: return "FixedJoint_computeChildBiasAcc";
-  case 932: return "FixedJoint_computeJointTorque";
-  case 933: return "FixedJoint_setIndex";
-  case 934: return "FixedJoint_getIndex";
-  case 935: return "FixedJoint_setPosCoordsOffset";
-  case 936: return "FixedJoint_getPosCoordsOffset";
-  case 937: return "FixedJoint_setDOFsOffset";
-  case 938: return "FixedJoint_getDOFsOffset";
-  case 939: return "FixedJoint_hasPosLimits";
-  case 940: return "FixedJoint_enablePosLimits";
-  case 941: return "FixedJoint_getPosLimits";
-  case 942: return "FixedJoint_getMinPosLimit";
-  case 943: return "FixedJoint_getMaxPosLimit";
-  case 944: return "FixedJoint_setPosLimits";
-  case 945: return "delete_MovableJointImpl1";
-  case 946: return "MovableJointImpl1_getNrOfPosCoords";
-  case 947: return "MovableJointImpl1_getNrOfDOFs";
-  case 948: return "MovableJointImpl1_setIndex";
-  case 949: return "MovableJointImpl1_getIndex";
-  case 950: return "MovableJointImpl1_setPosCoordsOffset";
-  case 951: return "MovableJointImpl1_getPosCoordsOffset";
-  case 952: return "MovableJointImpl1_setDOFsOffset";
-  case 953: return "MovableJointImpl1_getDOFsOffset";
-  case 954: return "delete_MovableJointImpl2";
-  case 955: return "MovableJointImpl2_getNrOfPosCoords";
-  case 956: return "MovableJointImpl2_getNrOfDOFs";
-  case 957: return "MovableJointImpl2_setIndex";
-  case 958: return "MovableJointImpl2_getIndex";
-  case 959: return "MovableJointImpl2_setPosCoordsOffset";
-  case 960: return "MovableJointImpl2_getPosCoordsOffset";
-  case 961: return "MovableJointImpl2_setDOFsOffset";
-  case 962: return "MovableJointImpl2_getDOFsOffset";
-  case 963: return "delete_MovableJointImpl3";
-  case 964: return "MovableJointImpl3_getNrOfPosCoords";
-  case 965: return "MovableJointImpl3_getNrOfDOFs";
-  case 966: return "MovableJointImpl3_setIndex";
-  case 967: return "MovableJointImpl3_getIndex";
-  case 968: return "MovableJointImpl3_setPosCoordsOffset";
-  case 969: return "MovableJointImpl3_getPosCoordsOffset";
-  case 970: return "MovableJointImpl3_setDOFsOffset";
-  case 971: return "MovableJointImpl3_getDOFsOffset";
-  case 972: return "delete_MovableJointImpl4";
-  case 973: return "MovableJointImpl4_getNrOfPosCoords";
-  case 974: return "MovableJointImpl4_getNrOfDOFs";
-  case 975: return "MovableJointImpl4_setIndex";
-  case 976: return "MovableJointImpl4_getIndex";
-  case 977: return "MovableJointImpl4_setPosCoordsOffset";
-  case 978: return "MovableJointImpl4_getPosCoordsOffset";
-  case 979: return "MovableJointImpl4_setDOFsOffset";
-  case 980: return "MovableJointImpl4_getDOFsOffset";
-  case 981: return "delete_MovableJointImpl5";
-  case 982: return "MovableJointImpl5_getNrOfPosCoords";
-  case 983: return "MovableJointImpl5_getNrOfDOFs";
-  case 984: return "MovableJointImpl5_setIndex";
-  case 985: return "MovableJointImpl5_getIndex";
-  case 986: return "MovableJointImpl5_setPosCoordsOffset";
-  case 987: return "MovableJointImpl5_getPosCoordsOffset";
-  case 988: return "MovableJointImpl5_setDOFsOffset";
-  case 989: return "MovableJointImpl5_getDOFsOffset";
-  case 990: return "delete_MovableJointImpl6";
-  case 991: return "MovableJointImpl6_getNrOfPosCoords";
-  case 992: return "MovableJointImpl6_getNrOfDOFs";
-  case 993: return "MovableJointImpl6_setIndex";
-  case 994: return "MovableJointImpl6_getIndex";
-  case 995: return "MovableJointImpl6_setPosCoordsOffset";
-  case 996: return "MovableJointImpl6_getPosCoordsOffset";
-  case 997: return "MovableJointImpl6_setDOFsOffset";
-  case 998: return "MovableJointImpl6_getDOFsOffset";
-  case 999: return "new_RevoluteJoint";
-  case 1000: return "delete_RevoluteJoint";
-  case 1001: return "RevoluteJoint_clone";
-  case 1002: return "RevoluteJoint_setAttachedLinks";
-  case 1003: return "RevoluteJoint_setRestTransform";
-  case 1004: return "RevoluteJoint_setAxis";
-  case 1005: return "RevoluteJoint_getFirstAttachedLink";
-  case 1006: return "RevoluteJoint_getSecondAttachedLink";
-  case 1007: return "RevoluteJoint_getAxis";
-  case 1008: return "RevoluteJoint_getRestTransform";
-  case 1009: return "RevoluteJoint_getTransform";
-  case 1010: return "RevoluteJoint_getTransformDerivative";
-  case 1011: return "RevoluteJoint_getMotionSubspaceVector";
-  case 1012: return "RevoluteJoint_computeChildPosVelAcc";
-  case 1013: return "RevoluteJoint_computeChildVel";
-  case 1014: return "RevoluteJoint_computeChildVelAcc";
-  case 1015: return "RevoluteJoint_computeChildAcc";
-  case 1016: return "RevoluteJoint_computeChildBiasAcc";
-  case 1017: return "RevoluteJoint_computeJointTorque";
-  case 1018: return "RevoluteJoint_hasPosLimits";
-  case 1019: return "RevoluteJoint_enablePosLimits";
-  case 1020: return "RevoluteJoint_getPosLimits";
-  case 1021: return "RevoluteJoint_getMinPosLimit";
-  case 1022: return "RevoluteJoint_getMaxPosLimit";
-  case 1023: return "RevoluteJoint_setPosLimits";
-  case 1024: return "new_PrismaticJoint";
-  case 1025: return "delete_PrismaticJoint";
-  case 1026: return "PrismaticJoint_clone";
-  case 1027: return "PrismaticJoint_setAttachedLinks";
-  case 1028: return "PrismaticJoint_setRestTransform";
-  case 1029: return "PrismaticJoint_setAxis";
-  case 1030: return "PrismaticJoint_getFirstAttachedLink";
-  case 1031: return "PrismaticJoint_getSecondAttachedLink";
-  case 1032: return "PrismaticJoint_getAxis";
-  case 1033: return "PrismaticJoint_getRestTransform";
-  case 1034: return "PrismaticJoint_getTransform";
-  case 1035: return "PrismaticJoint_getTransformDerivative";
-  case 1036: return "PrismaticJoint_getMotionSubspaceVector";
-  case 1037: return "PrismaticJoint_computeChildPosVelAcc";
-  case 1038: return "PrismaticJoint_computeChildVel";
-  case 1039: return "PrismaticJoint_computeChildVelAcc";
-  case 1040: return "PrismaticJoint_computeChildAcc";
-  case 1041: return "PrismaticJoint_computeChildBiasAcc";
-  case 1042: return "PrismaticJoint_computeJointTorque";
-  case 1043: return "PrismaticJoint_hasPosLimits";
-  case 1044: return "PrismaticJoint_enablePosLimits";
-  case 1045: return "PrismaticJoint_getPosLimits";
-  case 1046: return "PrismaticJoint_getMinPosLimit";
-  case 1047: return "PrismaticJoint_getMaxPosLimit";
-  case 1048: return "PrismaticJoint_setPosLimits";
-  case 1049: return "new_Traversal";
-  case 1050: return "delete_Traversal";
-  case 1051: return "Traversal_getNrOfVisitedLinks";
-  case 1052: return "Traversal_getLink";
-  case 1053: return "Traversal_getBaseLink";
-  case 1054: return "Traversal_getParentLink";
-  case 1055: return "Traversal_getParentJoint";
-  case 1056: return "Traversal_getParentLinkFromLinkIndex";
-  case 1057: return "Traversal_getParentJointFromLinkIndex";
-  case 1058: return "Traversal_getTraversalIndexFromLinkIndex";
-  case 1059: return "Traversal_reset";
-  case 1060: return "Traversal_addTraversalBase";
-  case 1061: return "Traversal_addTraversalElement";
-  case 1062: return "Traversal_isParentOf";
-  case 1063: return "Traversal_getChildLinkIndexFromJointIndex";
-  case 1064: return "Traversal_getParentLinkIndexFromJointIndex";
-  case 1065: return "Traversal_toString";
-  case 1066: return "delete_SolidShape";
-  case 1067: return "SolidShape_clone";
-  case 1068: return "SolidShape_name_get";
-  case 1069: return "SolidShape_name_set";
-  case 1070: return "SolidShape_link_H_geometry_get";
-  case 1071: return "SolidShape_link_H_geometry_set";
-  case 1072: return "SolidShape_material_get";
-  case 1073: return "SolidShape_material_set";
-  case 1074: return "SolidShape_isSphere";
-  case 1075: return "SolidShape_isBox";
-  case 1076: return "SolidShape_isCylinder";
-  case 1077: return "SolidShape_isExternalMesh";
-  case 1078: return "SolidShape_asSphere";
-  case 1079: return "SolidShape_asBox";
-  case 1080: return "SolidShape_asCylinder";
-  case 1081: return "SolidShape_asExternalMesh";
-  case 1082: return "delete_Sphere";
-  case 1083: return "Sphere_clone";
-  case 1084: return "Sphere_radius_get";
-  case 1085: return "Sphere_radius_set";
-  case 1086: return "new_Sphere";
-  case 1087: return "delete_Box";
-  case 1088: return "Box_clone";
-  case 1089: return "Box_x_get";
-  case 1090: return "Box_x_set";
-  case 1091: return "Box_y_get";
-  case 1092: return "Box_y_set";
-  case 1093: return "Box_z_get";
-  case 1094: return "Box_z_set";
-  case 1095: return "new_Box";
-  case 1096: return "delete_Cylinder";
-  case 1097: return "Cylinder_clone";
-  case 1098: return "Cylinder_length_get";
-  case 1099: return "Cylinder_length_set";
-  case 1100: return "Cylinder_radius_get";
-  case 1101: return "Cylinder_radius_set";
-  case 1102: return "new_Cylinder";
-  case 1103: return "delete_ExternalMesh";
-  case 1104: return "ExternalMesh_clone";
-  case 1105: return "ExternalMesh_filename_get";
-  case 1106: return "ExternalMesh_filename_set";
-  case 1107: return "ExternalMesh_scale_get";
-  case 1108: return "ExternalMesh_scale_set";
-  case 1109: return "new_ExternalMesh";
-  case 1110: return "new_ModelSolidShapes";
-  case 1111: return "ModelSolidShapes_clear";
-  case 1112: return "delete_ModelSolidShapes";
-  case 1113: return "ModelSolidShapes_resize";
-  case 1114: return "ModelSolidShapes_isConsistent";
-  case 1115: return "ModelSolidShapes_linkSolidShapes_get";
-  case 1116: return "ModelSolidShapes_linkSolidShapes_set";
-  case 1117: return "Neighbor_neighborLink_get";
-  case 1118: return "Neighbor_neighborLink_set";
-  case 1119: return "Neighbor_neighborJoint_get";
-  case 1120: return "Neighbor_neighborJoint_set";
-  case 1121: return "new_Neighbor";
-  case 1122: return "delete_Neighbor";
-  case 1123: return "new_Model";
-  case 1124: return "Model_copy";
-  case 1125: return "delete_Model";
-  case 1126: return "Model_getNrOfLinks";
-  case 1127: return "Model_getLinkName";
-  case 1128: return "Model_getLinkIndex";
-  case 1129: return "Model_isValidLinkIndex";
-  case 1130: return "Model_getLink";
-  case 1131: return "Model_addLink";
-  case 1132: return "Model_getNrOfJoints";
-  case 1133: return "Model_getJointName";
-  case 1134: return "Model_getJointIndex";
-  case 1135: return "Model_getJoint";
-  case 1136: return "Model_isValidJointIndex";
-  case 1137: return "Model_isLinkNameUsed";
-  case 1138: return "Model_isJointNameUsed";
-  case 1139: return "Model_isFrameNameUsed";
-  case 1140: return "Model_addJoint";
-  case 1141: return "Model_addJointAndLink";
-  case 1142: return "Model_insertLinkToExistingJointAndAddJointForDisplacedLink";
-  case 1143: return "Model_getNrOfPosCoords";
-  case 1144: return "Model_getNrOfDOFs";
-  case 1145: return "Model_getNrOfFrames";
-  case 1146: return "Model_addAdditionalFrameToLink";
-  case 1147: return "Model_getFrameName";
-  case 1148: return "Model_getFrameIndex";
-  case 1149: return "Model_isValidFrameIndex";
-  case 1150: return "Model_getFrameTransform";
-  case 1151: return "Model_getFrameLink";
-  case 1152: return "Model_getNrOfNeighbors";
-  case 1153: return "Model_getNeighbor";
-  case 1154: return "Model_setDefaultBaseLink";
-  case 1155: return "Model_getDefaultBaseLink";
-  case 1156: return "Model_computeFullTreeTraversal";
-  case 1157: return "Model_getInertialParameters";
-  case 1158: return "Model_updateInertialParameters";
-  case 1159: return "Model_visualSolidShapes";
-  case 1160: return "Model_collisionSolidShapes";
-  case 1161: return "Model_toString";
-  case 1162: return "new_JointPosDoubleArray";
-  case 1163: return "JointPosDoubleArray_resize";
-  case 1164: return "JointPosDoubleArray_isConsistent";
-  case 1165: return "delete_JointPosDoubleArray";
-  case 1166: return "new_JointDOFsDoubleArray";
-  case 1167: return "JointDOFsDoubleArray_resize";
-  case 1168: return "JointDOFsDoubleArray_isConsistent";
-  case 1169: return "delete_JointDOFsDoubleArray";
-  case 1170: return "new_DOFSpatialForceArray";
-  case 1171: return "DOFSpatialForceArray_resize";
-  case 1172: return "DOFSpatialForceArray_isConsistent";
-  case 1173: return "DOFSpatialForceArray_paren";
-  case 1174: return "delete_DOFSpatialForceArray";
-  case 1175: return "new_DOFSpatialMotionArray";
-  case 1176: return "DOFSpatialMotionArray_resize";
-  case 1177: return "DOFSpatialMotionArray_isConsistent";
-  case 1178: return "DOFSpatialMotionArray_paren";
-  case 1179: return "delete_DOFSpatialMotionArray";
-  case 1180: return "new_FrameFreeFloatingJacobian";
-  case 1181: return "FrameFreeFloatingJacobian_resize";
-  case 1182: return "FrameFreeFloatingJacobian_isConsistent";
-  case 1183: return "delete_FrameFreeFloatingJacobian";
-  case 1184: return "new_MomentumFreeFloatingJacobian";
-  case 1185: return "MomentumFreeFloatingJacobian_resize";
-  case 1186: return "MomentumFreeFloatingJacobian_isConsistent";
-  case 1187: return "delete_MomentumFreeFloatingJacobian";
-  case 1188: return "new_FreeFloatingMassMatrix";
-  case 1189: return "FreeFloatingMassMatrix_resize";
-  case 1190: return "delete_FreeFloatingMassMatrix";
-  case 1191: return "new_FreeFloatingPos";
-  case 1192: return "FreeFloatingPos_resize";
-  case 1193: return "FreeFloatingPos_worldBasePos";
-  case 1194: return "FreeFloatingPos_jointPos";
-  case 1195: return "FreeFloatingPos_getNrOfPosCoords";
-  case 1196: return "delete_FreeFloatingPos";
-  case 1197: return "new_FreeFloatingGeneralizedTorques";
-  case 1198: return "FreeFloatingGeneralizedTorques_resize";
-  case 1199: return "FreeFloatingGeneralizedTorques_baseWrench";
-  case 1200: return "FreeFloatingGeneralizedTorques_jointTorques";
-  case 1201: return "FreeFloatingGeneralizedTorques_getNrOfDOFs";
-  case 1202: return "delete_FreeFloatingGeneralizedTorques";
-  case 1203: return "new_FreeFloatingVel";
-  case 1204: return "FreeFloatingVel_resize";
-  case 1205: return "FreeFloatingVel_baseVel";
-  case 1206: return "FreeFloatingVel_jointVel";
-  case 1207: return "FreeFloatingVel_getNrOfDOFs";
-  case 1208: return "delete_FreeFloatingVel";
-  case 1209: return "new_FreeFloatingAcc";
-  case 1210: return "FreeFloatingAcc_resize";
-  case 1211: return "FreeFloatingAcc_baseAcc";
-  case 1212: return "FreeFloatingAcc_jointAcc";
-  case 1213: return "FreeFloatingAcc_getNrOfDOFs";
-  case 1214: return "delete_FreeFloatingAcc";
-  case 1215: return "ContactWrench_contactId";
-  case 1216: return "ContactWrench_contactPoint";
-  case 1217: return "ContactWrench_contactWrench";
-  case 1218: return "new_ContactWrench";
-  case 1219: return "delete_ContactWrench";
-  case 1220: return "new_LinkContactWrenches";
-  case 1221: return "LinkContactWrenches_resize";
-  case 1222: return "LinkContactWrenches_getNrOfContactsForLink";
-  case 1223: return "LinkContactWrenches_setNrOfContactsForLink";
-  case 1224: return "LinkContactWrenches_getNrOfLinks";
-  case 1225: return "LinkContactWrenches_contactWrench";
-  case 1226: return "LinkContactWrenches_computeNetWrenches";
-  case 1227: return "LinkContactWrenches_toString";
-  case 1228: return "delete_LinkContactWrenches";
-  case 1229: return "_wrap_ForwardPositionKinematics";
-  case 1230: return "_wrap_ForwardVelAccKinematics";
-  case 1231: return "_wrap_ForwardPosVelAccKinematics";
-  case 1232: return "_wrap_ForwardPosVelKinematics";
-  case 1233: return "_wrap_ForwardAccKinematics";
-  case 1234: return "_wrap_ForwardBiasAccKinematics";
-  case 1235: return "_wrap_ComputeLinearAndAngularMomentum";
-  case 1236: return "_wrap_ComputeLinearAndAngularMomentumDerivativeBias";
-  case 1237: return "_wrap_RNEADynamicPhase";
-  case 1238: return "_wrap_CompositeRigidBodyAlgorithm";
-  case 1239: return "new_ArticulatedBodyAlgorithmInternalBuffers";
-  case 1240: return "ArticulatedBodyAlgorithmInternalBuffers_resize";
-  case 1241: return "ArticulatedBodyAlgorithmInternalBuffers_isConsistent";
-  case 1242: return "ArticulatedBodyAlgorithmInternalBuffers_S_get";
-  case 1243: return "ArticulatedBodyAlgorithmInternalBuffers_S_set";
-  case 1244: return "ArticulatedBodyAlgorithmInternalBuffers_U_get";
-  case 1245: return "ArticulatedBodyAlgorithmInternalBuffers_U_set";
-  case 1246: return "ArticulatedBodyAlgorithmInternalBuffers_D_get";
-  case 1247: return "ArticulatedBodyAlgorithmInternalBuffers_D_set";
-  case 1248: return "ArticulatedBodyAlgorithmInternalBuffers_u_get";
-  case 1249: return "ArticulatedBodyAlgorithmInternalBuffers_u_set";
-  case 1250: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_get";
-  case 1251: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set";
-  case 1252: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get";
-  case 1253: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set";
-  case 1254: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get";
-  case 1255: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set";
-  case 1256: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get";
-  case 1257: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set";
-  case 1258: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get";
-  case 1259: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set";
-  case 1260: return "delete_ArticulatedBodyAlgorithmInternalBuffers";
-  case 1261: return "_wrap_ArticulatedBodyAlgorithm";
-  case 1262: return "NR_OF_SENSOR_TYPES_get";
-  case 1263: return "_wrap_isLinkSensor";
-  case 1264: return "_wrap_isJointSensor";
-  case 1265: return "_wrap_getSensorTypeSize";
-  case 1266: return "delete_Sensor";
-  case 1267: return "Sensor_getName";
-  case 1268: return "Sensor_getSensorType";
-  case 1269: return "Sensor_isValid";
-  case 1270: return "Sensor_setName";
-  case 1271: return "Sensor_clone";
-  case 1272: return "Sensor_isConsistent";
-  case 1273: return "Sensor_updateIndices";
-  case 1274: return "Sensor_updateIndeces";
-  case 1275: return "delete_JointSensor";
-  case 1276: return "JointSensor_getParentJoint";
-  case 1277: return "JointSensor_getParentJointIndex";
-  case 1278: return "JointSensor_setParentJoint";
-  case 1279: return "JointSensor_setParentJointIndex";
-  case 1280: return "JointSensor_isConsistent";
-  case 1281: return "delete_LinkSensor";
-  case 1282: return "LinkSensor_getParentLink";
-  case 1283: return "LinkSensor_getParentLinkIndex";
-  case 1284: return "LinkSensor_getLinkSensorTransform";
-  case 1285: return "LinkSensor_setParentLink";
-  case 1286: return "LinkSensor_setParentLinkIndex";
-  case 1287: return "LinkSensor_setLinkSensorTransform";
-  case 1288: return "LinkSensor_isConsistent";
-  case 1289: return "new_SensorsList";
-  case 1290: return "delete_SensorsList";
-  case 1291: return "SensorsList_addSensor";
-  case 1292: return "SensorsList_setSerialization";
-  case 1293: return "SensorsList_getSerialization";
-  case 1294: return "SensorsList_getNrOfSensors";
-  case 1295: return "SensorsList_getSensorIndex";
-  case 1296: return "SensorsList_getSizeOfAllSensorsMeasurements";
-  case 1297: return "SensorsList_getSensor";
-  case 1298: return "SensorsList_isConsistent";
-  case 1299: return "SensorsList_removeSensor";
-  case 1300: return "SensorsList_removeAllSensorsOfType";
-  case 1301: return "SensorsList_getSixAxisForceTorqueSensor";
-  case 1302: return "SensorsList_getAccelerometerSensor";
-  case 1303: return "SensorsList_getGyroscopeSensor";
-  case 1304: return "SensorsList_getThreeAxisAngularAccelerometerSensor";
-  case 1305: return "SensorsList_getThreeAxisForceTorqueContactSensor";
-  case 1306: return "new_SensorsMeasurements";
-  case 1307: return "delete_SensorsMeasurements";
-  case 1308: return "SensorsMeasurements_setNrOfSensors";
-  case 1309: return "SensorsMeasurements_getNrOfSensors";
-  case 1310: return "SensorsMeasurements_resize";
-  case 1311: return "SensorsMeasurements_toVector";
-  case 1312: return "SensorsMeasurements_setMeasurement";
-  case 1313: return "SensorsMeasurements_getMeasurement";
-  case 1314: return "SensorsMeasurements_getSizeOfAllSensorsMeasurements";
-  case 1315: return "new_SixAxisForceTorqueSensor";
-  case 1316: return "delete_SixAxisForceTorqueSensor";
-  case 1317: return "SixAxisForceTorqueSensor_setName";
-  case 1318: return "SixAxisForceTorqueSensor_setFirstLinkSensorTransform";
-  case 1319: return "SixAxisForceTorqueSensor_setSecondLinkSensorTransform";
-  case 1320: return "SixAxisForceTorqueSensor_getFirstLinkIndex";
-  case 1321: return "SixAxisForceTorqueSensor_getSecondLinkIndex";
-  case 1322: return "SixAxisForceTorqueSensor_setFirstLinkName";
-  case 1323: return "SixAxisForceTorqueSensor_setSecondLinkName";
-  case 1324: return "SixAxisForceTorqueSensor_getFirstLinkName";
-  case 1325: return "SixAxisForceTorqueSensor_getSecondLinkName";
-  case 1326: return "SixAxisForceTorqueSensor_setParentJoint";
-  case 1327: return "SixAxisForceTorqueSensor_setParentJointIndex";
-  case 1328: return "SixAxisForceTorqueSensor_setAppliedWrenchLink";
-  case 1329: return "SixAxisForceTorqueSensor_getName";
-  case 1330: return "SixAxisForceTorqueSensor_getSensorType";
-  case 1331: return "SixAxisForceTorqueSensor_getParentJoint";
-  case 1332: return "SixAxisForceTorqueSensor_getParentJointIndex";
-  case 1333: return "SixAxisForceTorqueSensor_isValid";
-  case 1334: return "SixAxisForceTorqueSensor_clone";
-  case 1335: return "SixAxisForceTorqueSensor_updateIndices";
-  case 1336: return "SixAxisForceTorqueSensor_updateIndeces";
-  case 1337: return "SixAxisForceTorqueSensor_getAppliedWrenchLink";
-  case 1338: return "SixAxisForceTorqueSensor_isLinkAttachedToSensor";
-  case 1339: return "SixAxisForceTorqueSensor_getLinkSensorTransform";
-  case 1340: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLink";
-  case 1341: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix";
-  case 1342: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix";
-  case 1343: return "SixAxisForceTorqueSensor_predictMeasurement";
-  case 1344: return "SixAxisForceTorqueSensor_toString";
-  case 1345: return "new_AccelerometerSensor";
-  case 1346: return "delete_AccelerometerSensor";
-  case 1347: return "AccelerometerSensor_setName";
-  case 1348: return "AccelerometerSensor_setLinkSensorTransform";
-  case 1349: return "AccelerometerSensor_setParentLink";
-  case 1350: return "AccelerometerSensor_setParentLinkIndex";
-  case 1351: return "AccelerometerSensor_getName";
-  case 1352: return "AccelerometerSensor_getSensorType";
-  case 1353: return "AccelerometerSensor_getParentLink";
-  case 1354: return "AccelerometerSensor_getParentLinkIndex";
-  case 1355: return "AccelerometerSensor_getLinkSensorTransform";
-  case 1356: return "AccelerometerSensor_isValid";
-  case 1357: return "AccelerometerSensor_clone";
-  case 1358: return "AccelerometerSensor_updateIndices";
-  case 1359: return "AccelerometerSensor_updateIndeces";
-  case 1360: return "AccelerometerSensor_predictMeasurement";
-  case 1361: return "new_GyroscopeSensor";
-  case 1362: return "delete_GyroscopeSensor";
-  case 1363: return "GyroscopeSensor_setName";
-  case 1364: return "GyroscopeSensor_setLinkSensorTransform";
-  case 1365: return "GyroscopeSensor_setParentLink";
-  case 1366: return "GyroscopeSensor_setParentLinkIndex";
-  case 1367: return "GyroscopeSensor_getName";
-  case 1368: return "GyroscopeSensor_getSensorType";
-  case 1369: return "GyroscopeSensor_getParentLink";
-  case 1370: return "GyroscopeSensor_getParentLinkIndex";
-  case 1371: return "GyroscopeSensor_getLinkSensorTransform";
-  case 1372: return "GyroscopeSensor_isValid";
-  case 1373: return "GyroscopeSensor_clone";
-  case 1374: return "GyroscopeSensor_updateIndices";
-  case 1375: return "GyroscopeSensor_updateIndeces";
-  case 1376: return "GyroscopeSensor_predictMeasurement";
-  case 1377: return "new_ThreeAxisAngularAccelerometerSensor";
-  case 1378: return "delete_ThreeAxisAngularAccelerometerSensor";
-  case 1379: return "ThreeAxisAngularAccelerometerSensor_setName";
-  case 1380: return "ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform";
-  case 1381: return "ThreeAxisAngularAccelerometerSensor_setParentLink";
-  case 1382: return "ThreeAxisAngularAccelerometerSensor_setParentLinkIndex";
-  case 1383: return "ThreeAxisAngularAccelerometerSensor_getName";
-  case 1384: return "ThreeAxisAngularAccelerometerSensor_getSensorType";
-  case 1385: return "ThreeAxisAngularAccelerometerSensor_getParentLink";
-  case 1386: return "ThreeAxisAngularAccelerometerSensor_getParentLinkIndex";
-  case 1387: return "ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform";
-  case 1388: return "ThreeAxisAngularAccelerometerSensor_isValid";
-  case 1389: return "ThreeAxisAngularAccelerometerSensor_clone";
-  case 1390: return "ThreeAxisAngularAccelerometerSensor_updateIndices";
-  case 1391: return "ThreeAxisAngularAccelerometerSensor_predictMeasurement";
-  case 1392: return "new_ThreeAxisForceTorqueContactSensor";
-  case 1393: return "delete_ThreeAxisForceTorqueContactSensor";
-  case 1394: return "ThreeAxisForceTorqueContactSensor_setName";
-  case 1395: return "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform";
-  case 1396: return "ThreeAxisForceTorqueContactSensor_setParentLink";
-  case 1397: return "ThreeAxisForceTorqueContactSensor_setParentLinkIndex";
-  case 1398: return "ThreeAxisForceTorqueContactSensor_getName";
-  case 1399: return "ThreeAxisForceTorqueContactSensor_getSensorType";
-  case 1400: return "ThreeAxisForceTorqueContactSensor_getParentLink";
-  case 1401: return "ThreeAxisForceTorqueContactSensor_getParentLinkIndex";
-  case 1402: return "ThreeAxisForceTorqueContactSensor_getLinkSensorTransform";
-  case 1403: return "ThreeAxisForceTorqueContactSensor_isValid";
-  case 1404: return "ThreeAxisForceTorqueContactSensor_clone";
-  case 1405: return "ThreeAxisForceTorqueContactSensor_updateIndices";
-  case 1406: return "ThreeAxisForceTorqueContactSensor_setLoadCellLocations";
-  case 1407: return "ThreeAxisForceTorqueContactSensor_getLoadCellLocations";
-  case 1408: return "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements";
-  case 1409: return "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements";
-  case 1410: return "_wrap_predictSensorsMeasurements";
-  case 1411: return "_wrap_predictSensorsMeasurementsFromRawBuffers";
-  case 1412: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_get";
-  case 1413: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_set";
-  case 1414: return "URDFParserOptions_originalFilename_get";
-  case 1415: return "URDFParserOptions_originalFilename_set";
-  case 1416: return "new_URDFParserOptions";
-  case 1417: return "delete_URDFParserOptions";
-  case 1418: return "_wrap_modelFromURDF";
-  case 1419: return "_wrap_modelFromURDFString";
-  case 1420: return "_wrap_dofsListFromURDF";
-  case 1421: return "_wrap_dofsListFromURDFString";
-  case 1422: return "_wrap_sensorsFromURDF";
-  case 1423: return "_wrap_sensorsFromURDFString";
-  case 1424: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_get";
-  case 1425: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_set";
-  case 1426: return "ModelParserOptions_originalFilename_get";
-  case 1427: return "ModelParserOptions_originalFilename_set";
-  case 1428: return "new_ModelParserOptions";
-  case 1429: return "delete_ModelParserOptions";
-  case 1430: return "new_ModelLoader";
-  case 1431: return "delete_ModelLoader";
-  case 1432: return "ModelLoader_parsingOptions";
-  case 1433: return "ModelLoader_setParsingOptions";
-  case 1434: return "ModelLoader_loadModelFromString";
-  case 1435: return "ModelLoader_loadModelFromFile";
-  case 1436: return "ModelLoader_loadReducedModelFromFullModel";
-  case 1437: return "ModelLoader_loadReducedModelFromString";
-  case 1438: return "ModelLoader_loadReducedModelFromFile";
-  case 1439: return "ModelLoader_model";
-  case 1440: return "ModelLoader_sensors";
-  case 1441: return "ModelLoader_isValid";
-  case 1442: return "new_UnknownWrenchContact";
-  case 1443: return "UnknownWrenchContact_unknownType_get";
-  case 1444: return "UnknownWrenchContact_unknownType_set";
-  case 1445: return "UnknownWrenchContact_contactPoint_get";
-  case 1446: return "UnknownWrenchContact_contactPoint_set";
-  case 1447: return "UnknownWrenchContact_forceDirection_get";
-  case 1448: return "UnknownWrenchContact_forceDirection_set";
-  case 1449: return "UnknownWrenchContact_knownWrench_get";
-  case 1450: return "UnknownWrenchContact_knownWrench_set";
-  case 1451: return "UnknownWrenchContact_contactId_get";
-  case 1452: return "UnknownWrenchContact_contactId_set";
-  case 1453: return "delete_UnknownWrenchContact";
-  case 1454: return "new_LinkUnknownWrenchContacts";
-  case 1455: return "LinkUnknownWrenchContacts_clear";
-  case 1456: return "LinkUnknownWrenchContacts_resize";
-  case 1457: return "LinkUnknownWrenchContacts_getNrOfContactsForLink";
-  case 1458: return "LinkUnknownWrenchContacts_setNrOfContactsForLink";
-  case 1459: return "LinkUnknownWrenchContacts_addNewContactForLink";
-  case 1460: return "LinkUnknownWrenchContacts_addNewContactInFrame";
-  case 1461: return "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin";
-  case 1462: return "LinkUnknownWrenchContacts_contactWrench";
-  case 1463: return "LinkUnknownWrenchContacts_toString";
-  case 1464: return "delete_LinkUnknownWrenchContacts";
-  case 1465: return "new_estimateExternalWrenchesBuffers";
-  case 1466: return "estimateExternalWrenchesBuffers_resize";
-  case 1467: return "estimateExternalWrenchesBuffers_getNrOfSubModels";
-  case 1468: return "estimateExternalWrenchesBuffers_getNrOfLinks";
-  case 1469: return "estimateExternalWrenchesBuffers_isConsistent";
-  case 1470: return "estimateExternalWrenchesBuffers_A_get";
-  case 1471: return "estimateExternalWrenchesBuffers_A_set";
-  case 1472: return "estimateExternalWrenchesBuffers_x_get";
-  case 1473: return "estimateExternalWrenchesBuffers_x_set";
-  case 1474: return "estimateExternalWrenchesBuffers_b_get";
-  case 1475: return "estimateExternalWrenchesBuffers_b_set";
-  case 1476: return "estimateExternalWrenchesBuffers_pinvA_get";
-  case 1477: return "estimateExternalWrenchesBuffers_pinvA_set";
-  case 1478: return "estimateExternalWrenchesBuffers_b_contacts_subtree_get";
-  case 1479: return "estimateExternalWrenchesBuffers_b_contacts_subtree_set";
-  case 1480: return "estimateExternalWrenchesBuffers_subModelBase_H_link_get";
-  case 1481: return "estimateExternalWrenchesBuffers_subModelBase_H_link_set";
-  case 1482: return "delete_estimateExternalWrenchesBuffers";
-  case 1483: return "_wrap_estimateExternalWrenchesWithoutInternalFT";
-  case 1484: return "_wrap_estimateExternalWrenches";
-  case 1485: return "_wrap_dynamicsEstimationForwardVelAccKinematics";
-  case 1486: return "_wrap_dynamicsEstimationForwardVelKinematics";
-  case 1487: return "_wrap_computeLinkNetWrenchesWithoutGravity";
-  case 1488: return "_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches";
-  case 1489: return "new_ExtWrenchesAndJointTorquesEstimator";
-  case 1490: return "delete_ExtWrenchesAndJointTorquesEstimator";
-  case 1491: return "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors";
-  case 1492: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile";
-  case 1493: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs";
-  case 1494: return "ExtWrenchesAndJointTorquesEstimator_model";
-  case 1495: return "ExtWrenchesAndJointTorquesEstimator_sensors";
-  case 1496: return "ExtWrenchesAndJointTorquesEstimator_submodels";
-  case 1497: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase";
-  case 1498: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase";
-  case 1499: return "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements";
-  case 1500: return "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques";
-  case 1501: return "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill";
-  case 1502: return "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity";
-  case 1503: return "new_SimpleLeggedOdometry";
-  case 1504: return "delete_SimpleLeggedOdometry";
-  case 1505: return "SimpleLeggedOdometry_setModel";
-  case 1506: return "SimpleLeggedOdometry_loadModelFromFile";
-  case 1507: return "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs";
-  case 1508: return "SimpleLeggedOdometry_model";
-  case 1509: return "SimpleLeggedOdometry_updateKinematics";
-  case 1510: return "SimpleLeggedOdometry_init";
-  case 1511: return "SimpleLeggedOdometry_changeFixedFrame";
-  case 1512: return "SimpleLeggedOdometry_getCurrentFixedLink";
-  case 1513: return "SimpleLeggedOdometry_getWorldLinkTransform";
-  case 1514: return "_wrap_isLinkBerdyDynamicVariable";
-  case 1515: return "_wrap_isJointBerdyDynamicVariable";
-  case 1516: return "_wrap_isDOFBerdyDynamicVariable";
-  case 1517: return "new_BerdyOptions";
-  case 1518: return "BerdyOptions_berdyVariant_get";
-  case 1519: return "BerdyOptions_berdyVariant_set";
-  case 1520: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get";
-  case 1521: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set";
-  case 1522: return "BerdyOptions_includeAllJointAccelerationsAsSensors_get";
-  case 1523: return "BerdyOptions_includeAllJointAccelerationsAsSensors_set";
-  case 1524: return "BerdyOptions_includeAllJointTorquesAsSensors_get";
-  case 1525: return "BerdyOptions_includeAllJointTorquesAsSensors_set";
-  case 1526: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get";
-  case 1527: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set";
-  case 1528: return "BerdyOptions_includeFixedBaseExternalWrench_get";
-  case 1529: return "BerdyOptions_includeFixedBaseExternalWrench_set";
-  case 1530: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get";
-  case 1531: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set";
-  case 1532: return "BerdyOptions_baseLink_get";
-  case 1533: return "BerdyOptions_baseLink_set";
-  case 1534: return "BerdyOptions_checkConsistency";
-  case 1535: return "delete_BerdyOptions";
-  case 1536: return "BerdySensor_type_get";
-  case 1537: return "BerdySensor_type_set";
-  case 1538: return "BerdySensor_id_get";
-  case 1539: return "BerdySensor_id_set";
-  case 1540: return "BerdySensor_range_get";
-  case 1541: return "BerdySensor_range_set";
-  case 1542: return "BerdySensor_eq";
-  case 1543: return "BerdySensor_lt";
-  case 1544: return "new_BerdySensor";
-  case 1545: return "delete_BerdySensor";
-  case 1546: return "BerdyDynamicVariable_type_get";
-  case 1547: return "BerdyDynamicVariable_type_set";
-  case 1548: return "BerdyDynamicVariable_id_get";
-  case 1549: return "BerdyDynamicVariable_id_set";
-  case 1550: return "BerdyDynamicVariable_range_get";
-  case 1551: return "BerdyDynamicVariable_range_set";
-  case 1552: return "BerdyDynamicVariable_eq";
-  case 1553: return "BerdyDynamicVariable_lt";
-  case 1554: return "new_BerdyDynamicVariable";
-  case 1555: return "delete_BerdyDynamicVariable";
-  case 1556: return "new_BerdyHelper";
-  case 1557: return "BerdyHelper_dynamicTraversal";
-  case 1558: return "BerdyHelper_model";
-  case 1559: return "BerdyHelper_sensors";
-  case 1560: return "BerdyHelper_isValid";
-  case 1561: return "BerdyHelper_init";
-  case 1562: return "BerdyHelper_getOptions";
-  case 1563: return "BerdyHelper_getNrOfDynamicVariables";
-  case 1564: return "BerdyHelper_getNrOfDynamicEquations";
-  case 1565: return "BerdyHelper_getNrOfSensorsMeasurements";
-  case 1566: return "BerdyHelper_resizeAndZeroBerdyMatrices";
-  case 1567: return "BerdyHelper_getBerdyMatrices";
-  case 1568: return "BerdyHelper_getSensorsOrdering";
-  case 1569: return "BerdyHelper_getRangeSensorVariable";
-  case 1570: return "BerdyHelper_getRangeDOFSensorVariable";
-  case 1571: return "BerdyHelper_getRangeJointSensorVariable";
-  case 1572: return "BerdyHelper_getRangeLinkSensorVariable";
-  case 1573: return "BerdyHelper_getRangeLinkVariable";
-  case 1574: return "BerdyHelper_getRangeJointVariable";
-  case 1575: return "BerdyHelper_getRangeDOFVariable";
-  case 1576: return "BerdyHelper_getDynamicVariablesOrdering";
-  case 1577: return "BerdyHelper_serializeDynamicVariables";
-  case 1578: return "BerdyHelper_serializeSensorVariables";
-  case 1579: return "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA";
-  case 1580: return "BerdyHelper_extractJointTorquesFromDynamicVariables";
-  case 1581: return "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables";
-  case 1582: return "BerdyHelper_updateKinematicsFromFloatingBase";
-  case 1583: return "BerdyHelper_updateKinematicsFromFixedBase";
-  case 1584: return "BerdyHelper_updateKinematicsFromTraversalFixedBase";
-  case 1585: return "BerdyHelper_setNetExternalWrenchMeasurementFrame";
-  case 1586: return "BerdyHelper_getNetExternalWrenchMeasurementFrame";
-  case 1587: return "delete_BerdyHelper";
-  case 1588: return "new_BerdySparseMAPSolver";
-  case 1589: return "delete_BerdySparseMAPSolver";
-  case 1590: return "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance";
-  case 1591: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance";
-  case 1592: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue";
-  case 1593: return "BerdySparseMAPSolver_setMeasurementsPriorCovariance";
-  case 1594: return "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse";
-  case 1595: return "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse";
-  case 1596: return "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue";
-  case 1597: return "BerdySparseMAPSolver_measurementsPriorCovarianceInverse";
-  case 1598: return "BerdySparseMAPSolver_isValid";
-  case 1599: return "BerdySparseMAPSolver_initialize";
-  case 1600: return "BerdySparseMAPSolver_updateEstimateInformationFixedBase";
-  case 1601: return "BerdySparseMAPSolver_updateEstimateInformationFloatingBase";
-  case 1602: return "BerdySparseMAPSolver_doEstimate";
-  case 1603: return "BerdySparseMAPSolver_getLastEstimate";
-  case 1604: return "DynamicsRegressorParameter_category_get";
-  case 1605: return "DynamicsRegressorParameter_category_set";
-  case 1606: return "DynamicsRegressorParameter_elemIndex_get";
-  case 1607: return "DynamicsRegressorParameter_elemIndex_set";
-  case 1608: return "DynamicsRegressorParameter_type_get";
-  case 1609: return "DynamicsRegressorParameter_type_set";
-  case 1610: return "DynamicsRegressorParameter_lt";
-  case 1611: return "DynamicsRegressorParameter_eq";
-  case 1612: return "DynamicsRegressorParameter_ne";
-  case 1613: return "new_DynamicsRegressorParameter";
-  case 1614: return "delete_DynamicsRegressorParameter";
-  case 1615: return "DynamicsRegressorParametersList_parameters_get";
-  case 1616: return "DynamicsRegressorParametersList_parameters_set";
-  case 1617: return "DynamicsRegressorParametersList_getDescriptionOfParameter";
-  case 1618: return "DynamicsRegressorParametersList_addParam";
-  case 1619: return "DynamicsRegressorParametersList_addList";
-  case 1620: return "DynamicsRegressorParametersList_findParam";
-  case 1621: return "DynamicsRegressorParametersList_getNrOfParameters";
-  case 1622: return "new_DynamicsRegressorParametersList";
-  case 1623: return "delete_DynamicsRegressorParametersList";
-  case 1624: return "new_DynamicsRegressorGenerator";
-  case 1625: return "delete_DynamicsRegressorGenerator";
-  case 1626: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile";
-  case 1627: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString";
-  case 1628: return "DynamicsRegressorGenerator_loadRegressorStructureFromFile";
-  case 1629: return "DynamicsRegressorGenerator_loadRegressorStructureFromString";
-  case 1630: return "DynamicsRegressorGenerator_isValid";
-  case 1631: return "DynamicsRegressorGenerator_getNrOfParameters";
-  case 1632: return "DynamicsRegressorGenerator_getNrOfOutputs";
-  case 1633: return "DynamicsRegressorGenerator_getNrOfDegreesOfFreedom";
-  case 1634: return "DynamicsRegressorGenerator_getDescriptionOfParameter";
-  case 1635: return "DynamicsRegressorGenerator_getDescriptionOfParameters";
-  case 1636: return "DynamicsRegressorGenerator_getDescriptionOfOutput";
-  case 1637: return "DynamicsRegressorGenerator_getDescriptionOfOutputs";
-  case 1638: return "DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom";
-  case 1639: return "DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom";
-  case 1640: return "DynamicsRegressorGenerator_getDescriptionOfLink";
-  case 1641: return "DynamicsRegressorGenerator_getDescriptionOfLinks";
-  case 1642: return "DynamicsRegressorGenerator_getNrOfLinks";
-  case 1643: return "DynamicsRegressorGenerator_getNrOfFakeLinks";
-  case 1644: return "DynamicsRegressorGenerator_getBaseLinkName";
-  case 1645: return "DynamicsRegressorGenerator_getSensorsModel";
-  case 1646: return "DynamicsRegressorGenerator_setRobotState";
-  case 1647: return "DynamicsRegressorGenerator_getSensorsMeasurements";
-  case 1648: return "DynamicsRegressorGenerator_setTorqueSensorMeasurement";
-  case 1649: return "DynamicsRegressorGenerator_computeRegressor";
-  case 1650: return "DynamicsRegressorGenerator_getModelParameters";
-  case 1651: return "DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace";
-  case 1652: return "DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace";
-  case 1653: return "DynamicsRegressorGenerator_generate_random_regressors";
-  case 1654: return "new_KinDynComputations";
-  case 1655: return "delete_KinDynComputations";
-  case 1656: return "KinDynComputations_loadRobotModel";
-  case 1657: return "KinDynComputations_loadRobotModelFromFile";
-  case 1658: return "KinDynComputations_loadRobotModelFromString";
-  case 1659: return "KinDynComputations_isValid";
-  case 1660: return "KinDynComputations_setFrameVelocityRepresentation";
-  case 1661: return "KinDynComputations_getFrameVelocityRepresentation";
-  case 1662: return "KinDynComputations_getNrOfDegreesOfFreedom";
-  case 1663: return "KinDynComputations_getDescriptionOfDegreeOfFreedom";
-  case 1664: return "KinDynComputations_getDescriptionOfDegreesOfFreedom";
-  case 1665: return "KinDynComputations_getNrOfLinks";
-  case 1666: return "KinDynComputations_getNrOfFrames";
-  case 1667: return "KinDynComputations_getFloatingBase";
-  case 1668: return "KinDynComputations_setFloatingBase";
-  case 1669: return "KinDynComputations_model";
-  case 1670: return "KinDynComputations_getRobotModel";
-  case 1671: return "KinDynComputations_getRelativeJacobianSparsityPattern";
-  case 1672: return "KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern";
-  case 1673: return "KinDynComputations_setJointPos";
-  case 1674: return "KinDynComputations_setRobotState";
-  case 1675: return "KinDynComputations_getRobotState";
-  case 1676: return "KinDynComputations_getWorldBaseTransform";
-  case 1677: return "KinDynComputations_getBaseTwist";
-  case 1678: return "KinDynComputations_getJointPos";
-  case 1679: return "KinDynComputations_getJointVel";
-  case 1680: return "KinDynComputations_getModelVel";
-  case 1681: return "KinDynComputations_getFrameIndex";
-  case 1682: return "KinDynComputations_getFrameName";
-  case 1683: return "KinDynComputations_getWorldTransform";
-  case 1684: return "KinDynComputations_getRelativeTransformExplicit";
-  case 1685: return "KinDynComputations_getRelativeTransform";
-  case 1686: return "KinDynComputations_getFrameVel";
-  case 1687: return "KinDynComputations_getFrameFreeFloatingJacobian";
-  case 1688: return "KinDynComputations_getRelativeJacobian";
-  case 1689: return "KinDynComputations_getRelativeJacobianExplicit";
-  case 1690: return "KinDynComputations_getFrameBiasAcc";
-  case 1691: return "KinDynComputations_getCenterOfMassPosition";
-  case 1692: return "KinDynComputations_getCenterOfMassVelocity";
-  case 1693: return "KinDynComputations_getCenterOfMassJacobian";
-  case 1694: return "KinDynComputations_getCenterOfMassBiasAcc";
-  case 1695: return "KinDynComputations_getAverageVelocity";
-  case 1696: return "KinDynComputations_getAverageVelocityJacobian";
-  case 1697: return "KinDynComputations_getCentroidalAverageVelocity";
-  case 1698: return "KinDynComputations_getCentroidalAverageVelocityJacobian";
-  case 1699: return "KinDynComputations_getLinearAngularMomentum";
-  case 1700: return "KinDynComputations_getLinearAngularMomentumJacobian";
-  case 1701: return "KinDynComputations_getCentroidalTotalMomentum";
-  case 1702: return "KinDynComputations_getFreeFloatingMassMatrix";
-  case 1703: return "KinDynComputations_inverseDynamics";
-  case 1704: return "KinDynComputations_generalizedBiasForces";
-  case 1705: return "KinDynComputations_generalizedGravityForces";
-  case 1706: return "delete_ICamera";
-  case 1707: return "ICamera_setPosition";
-  case 1708: return "ICamera_setTarget";
-  case 1709: return "ICamera_setUpVector";
-  case 1710: return "ColorViz_r_get";
-  case 1711: return "ColorViz_r_set";
-  case 1712: return "ColorViz_g_get";
-  case 1713: return "ColorViz_g_set";
-  case 1714: return "ColorViz_b_get";
-  case 1715: return "ColorViz_b_set";
-  case 1716: return "ColorViz_a_get";
-  case 1717: return "ColorViz_a_set";
-  case 1718: return "new_ColorViz";
-  case 1719: return "delete_ColorViz";
-  case 1720: return "delete_ILight";
-  case 1721: return "ILight_getName";
-  case 1722: return "ILight_setType";
-  case 1723: return "ILight_getType";
-  case 1724: return "ILight_setPosition";
-  case 1725: return "ILight_getPosition";
-  case 1726: return "ILight_setDirection";
-  case 1727: return "ILight_getDirection";
-  case 1728: return "ILight_setAmbientColor";
-  case 1729: return "ILight_getAmbientColor";
-  case 1730: return "ILight_setSpecularColor";
-  case 1731: return "ILight_getSpecularColor";
-  case 1732: return "ILight_setDiffuseColor";
-  case 1733: return "ILight_getDiffuseColor";
-  case 1734: return "delete_IEnvironment";
-  case 1735: return "IEnvironment_getElements";
-  case 1736: return "IEnvironment_setElementVisibility";
-  case 1737: return "IEnvironment_setBackgroundColor";
-  case 1738: return "IEnvironment_setAmbientLight";
-  case 1739: return "IEnvironment_getLights";
-  case 1740: return "IEnvironment_addLight";
-  case 1741: return "IEnvironment_lightViz";
-  case 1742: return "IEnvironment_removeLight";
-  case 1743: return "delete_IJetsVisualization";
-  case 1744: return "IJetsVisualization_setJetsFrames";
-  case 1745: return "IJetsVisualization_getNrOfJets";
-  case 1746: return "IJetsVisualization_getJetDirection";
-  case 1747: return "IJetsVisualization_setJetDirection";
-  case 1748: return "IJetsVisualization_setJetColor";
-  case 1749: return "IJetsVisualization_setJetsDimensions";
-  case 1750: return "IJetsVisualization_setJetsIntensity";
-  case 1751: return "delete_IModelVisualization";
-  case 1752: return "IModelVisualization_setPositions";
-  case 1753: return "IModelVisualization_setLinkPositions";
-  case 1754: return "IModelVisualization_model";
-  case 1755: return "IModelVisualization_getInstanceName";
-  case 1756: return "IModelVisualization_setModelVisibility";
-  case 1757: return "IModelVisualization_setModelColor";
-  case 1758: return "IModelVisualization_resetModelColor";
-  case 1759: return "IModelVisualization_getLinkNames";
-  case 1760: return "IModelVisualization_setLinkVisibility";
-  case 1761: return "IModelVisualization_getFeatures";
-  case 1762: return "IModelVisualization_setFeatureVisibility";
-  case 1763: return "IModelVisualization_jets";
-  case 1764: return "VisualizerOptions_verbose_get";
-  case 1765: return "VisualizerOptions_verbose_set";
-  case 1766: return "VisualizerOptions_winWidth_get";
-  case 1767: return "VisualizerOptions_winWidth_set";
-  case 1768: return "VisualizerOptions_winHeight_get";
-  case 1769: return "VisualizerOptions_winHeight_set";
-  case 1770: return "VisualizerOptions_rootFrameArrowsDimension_get";
-  case 1771: return "VisualizerOptions_rootFrameArrowsDimension_set";
-  case 1772: return "new_VisualizerOptions";
-  case 1773: return "delete_VisualizerOptions";
-  case 1774: return "new_Visualizer";
-  case 1775: return "delete_Visualizer";
-  case 1776: return "Visualizer_init";
-  case 1777: return "Visualizer_getNrOfVisualizedModels";
-  case 1778: return "Visualizer_getModelInstanceName";
-  case 1779: return "Visualizer_getModelInstanceIndex";
-  case 1780: return "Visualizer_addModel";
-  case 1781: return "Visualizer_modelViz";
-  case 1782: return "Visualizer_camera";
-  case 1783: return "Visualizer_enviroment";
-  case 1784: return "Visualizer_run";
-  case 1785: return "Visualizer_draw";
-  case 1786: return "Visualizer_drawToFile";
-  case 1787: return "Visualizer_close";
-  case 1788: return "new_DynamicsComputations";
-  case 1789: return "delete_DynamicsComputations";
-  case 1790: return "DynamicsComputations_loadRobotModelFromFile";
-  case 1791: return "DynamicsComputations_loadRobotModelFromString";
-  case 1792: return "DynamicsComputations_isValid";
-  case 1793: return "DynamicsComputations_getNrOfDegreesOfFreedom";
-  case 1794: return "DynamicsComputations_getDescriptionOfDegreeOfFreedom";
-  case 1795: return "DynamicsComputations_getDescriptionOfDegreesOfFreedom";
-  case 1796: return "DynamicsComputations_getNrOfLinks";
-  case 1797: return "DynamicsComputations_getNrOfFrames";
-  case 1798: return "DynamicsComputations_getFloatingBase";
-  case 1799: return "DynamicsComputations_setFloatingBase";
-  case 1800: return "DynamicsComputations_setRobotState";
-  case 1801: return "DynamicsComputations_getWorldBaseTransform";
-  case 1802: return "DynamicsComputations_getBaseTwist";
-  case 1803: return "DynamicsComputations_getJointPos";
-  case 1804: return "DynamicsComputations_getJointVel";
-  case 1805: return "DynamicsComputations_getFrameIndex";
-  case 1806: return "DynamicsComputations_getFrameName";
-  case 1807: return "DynamicsComputations_getWorldTransform";
-  case 1808: return "DynamicsComputations_getRelativeTransform";
-  case 1809: return "DynamicsComputations_getFrameTwist";
-  case 1810: return "DynamicsComputations_getFrameTwistInWorldOrient";
-  case 1811: return "DynamicsComputations_getFrameProperSpatialAcceleration";
-  case 1812: return "DynamicsComputations_getLinkIndex";
-  case 1813: return "DynamicsComputations_getLinkInertia";
-  case 1814: return "DynamicsComputations_getJointIndex";
-  case 1815: return "DynamicsComputations_getJointName";
-  case 1816: return "DynamicsComputations_getJointLimits";
-  case 1817: return "DynamicsComputations_inverseDynamics";
-  case 1818: return "DynamicsComputations_getFreeFloatingMassMatrix";
-  case 1819: return "DynamicsComputations_getFrameJacobian";
-  case 1820: return "DynamicsComputations_getDynamicsRegressor";
-  case 1821: return "DynamicsComputations_getModelDynamicsParameters";
-  case 1822: return "DynamicsComputations_getCenterOfMass";
-  case 1823: return "DynamicsComputations_getCenterOfMassJacobian";
+  case 816: return "dynamic_extent_get";
+  case 817: return "DynamicSpan_extent_get";
+  case 818: return "new_DynamicSpan";
+  case 819: return "delete_DynamicSpan";
+  case 820: return "DynamicSpan_first";
+  case 821: return "DynamicSpan_last";
+  case 822: return "DynamicSpan_subspan";
+  case 823: return "DynamicSpan_size";
+  case 824: return "DynamicSpan_size_bytes";
+  case 825: return "DynamicSpan_empty";
+  case 826: return "DynamicSpan_brace";
+  case 827: return "DynamicSpan_getVal";
+  case 828: return "DynamicSpan_setVal";
+  case 829: return "DynamicSpan_at";
+  case 830: return "DynamicSpan_paren";
+  case 831: return "DynamicSpan_begin";
+  case 832: return "DynamicSpan_end";
+  case 833: return "DynamicSpan_cbegin";
+  case 834: return "DynamicSpan_cend";
+  case 835: return "DynamicSpan_rbegin";
+  case 836: return "DynamicSpan_rend";
+  case 837: return "DynamicSpan_crbegin";
+  case 838: return "DynamicSpan_crend";
+  case 839: return "LINK_INVALID_INDEX_get";
+  case 840: return "LINK_INVALID_INDEX_set";
+  case 841: return "LINK_INVALID_NAME_get";
+  case 842: return "LINK_INVALID_NAME_set";
+  case 843: return "JOINT_INVALID_INDEX_get";
+  case 844: return "JOINT_INVALID_INDEX_set";
+  case 845: return "JOINT_INVALID_NAME_get";
+  case 846: return "JOINT_INVALID_NAME_set";
+  case 847: return "DOF_INVALID_INDEX_get";
+  case 848: return "DOF_INVALID_INDEX_set";
+  case 849: return "DOF_INVALID_NAME_get";
+  case 850: return "DOF_INVALID_NAME_set";
+  case 851: return "FRAME_INVALID_INDEX_get";
+  case 852: return "FRAME_INVALID_INDEX_set";
+  case 853: return "FRAME_INVALID_NAME_get";
+  case 854: return "FRAME_INVALID_NAME_set";
+  case 855: return "TRAVERSAL_INVALID_INDEX_get";
+  case 856: return "TRAVERSAL_INVALID_INDEX_set";
+  case 857: return "new_LinkPositions";
+  case 858: return "LinkPositions_resize";
+  case 859: return "LinkPositions_isConsistent";
+  case 860: return "LinkPositions_getNrOfLinks";
+  case 861: return "LinkPositions_paren";
+  case 862: return "LinkPositions_toString";
+  case 863: return "delete_LinkPositions";
+  case 864: return "new_LinkWrenches";
+  case 865: return "LinkWrenches_resize";
+  case 866: return "LinkWrenches_isConsistent";
+  case 867: return "LinkWrenches_getNrOfLinks";
+  case 868: return "LinkWrenches_paren";
+  case 869: return "LinkWrenches_toString";
+  case 870: return "LinkWrenches_zero";
+  case 871: return "delete_LinkWrenches";
+  case 872: return "new_LinkInertias";
+  case 873: return "LinkInertias_resize";
+  case 874: return "LinkInertias_isConsistent";
+  case 875: return "LinkInertias_paren";
+  case 876: return "delete_LinkInertias";
+  case 877: return "new_LinkArticulatedBodyInertias";
+  case 878: return "LinkArticulatedBodyInertias_resize";
+  case 879: return "LinkArticulatedBodyInertias_isConsistent";
+  case 880: return "LinkArticulatedBodyInertias_paren";
+  case 881: return "delete_LinkArticulatedBodyInertias";
+  case 882: return "new_LinkVelArray";
+  case 883: return "LinkVelArray_resize";
+  case 884: return "LinkVelArray_isConsistent";
+  case 885: return "LinkVelArray_getNrOfLinks";
+  case 886: return "LinkVelArray_paren";
+  case 887: return "LinkVelArray_toString";
+  case 888: return "delete_LinkVelArray";
+  case 889: return "new_LinkAccArray";
+  case 890: return "LinkAccArray_resize";
+  case 891: return "LinkAccArray_isConsistent";
+  case 892: return "LinkAccArray_paren";
+  case 893: return "LinkAccArray_getNrOfLinks";
+  case 894: return "LinkAccArray_toString";
+  case 895: return "delete_LinkAccArray";
+  case 896: return "new_Link";
+  case 897: return "Link_inertia";
+  case 898: return "Link_setInertia";
+  case 899: return "Link_getInertia";
+  case 900: return "Link_setIndex";
+  case 901: return "Link_getIndex";
+  case 902: return "delete_Link";
+  case 903: return "delete_IJoint";
+  case 904: return "IJoint_clone";
+  case 905: return "IJoint_getNrOfPosCoords";
+  case 906: return "IJoint_getNrOfDOFs";
+  case 907: return "IJoint_setAttachedLinks";
+  case 908: return "IJoint_setRestTransform";
+  case 909: return "IJoint_getFirstAttachedLink";
+  case 910: return "IJoint_getSecondAttachedLink";
+  case 911: return "IJoint_getRestTransform";
+  case 912: return "IJoint_getTransform";
+  case 913: return "IJoint_getTransformDerivative";
+  case 914: return "IJoint_getMotionSubspaceVector";
+  case 915: return "IJoint_computeChildPosVelAcc";
+  case 916: return "IJoint_computeChildVelAcc";
+  case 917: return "IJoint_computeChildVel";
+  case 918: return "IJoint_computeChildAcc";
+  case 919: return "IJoint_computeChildBiasAcc";
+  case 920: return "IJoint_computeJointTorque";
+  case 921: return "IJoint_setIndex";
+  case 922: return "IJoint_getIndex";
+  case 923: return "IJoint_setPosCoordsOffset";
+  case 924: return "IJoint_getPosCoordsOffset";
+  case 925: return "IJoint_setDOFsOffset";
+  case 926: return "IJoint_getDOFsOffset";
+  case 927: return "IJoint_hasPosLimits";
+  case 928: return "IJoint_enablePosLimits";
+  case 929: return "IJoint_getPosLimits";
+  case 930: return "IJoint_getMinPosLimit";
+  case 931: return "IJoint_getMaxPosLimit";
+  case 932: return "IJoint_setPosLimits";
+  case 933: return "IJoint_isRevoluteJoint";
+  case 934: return "IJoint_isFixedJoint";
+  case 935: return "IJoint_asRevoluteJoint";
+  case 936: return "IJoint_asFixedJoint";
+  case 937: return "new_FixedJoint";
+  case 938: return "delete_FixedJoint";
+  case 939: return "FixedJoint_clone";
+  case 940: return "FixedJoint_getNrOfPosCoords";
+  case 941: return "FixedJoint_getNrOfDOFs";
+  case 942: return "FixedJoint_setAttachedLinks";
+  case 943: return "FixedJoint_setRestTransform";
+  case 944: return "FixedJoint_getFirstAttachedLink";
+  case 945: return "FixedJoint_getSecondAttachedLink";
+  case 946: return "FixedJoint_getRestTransform";
+  case 947: return "FixedJoint_getTransform";
+  case 948: return "FixedJoint_getTransformDerivative";
+  case 949: return "FixedJoint_getMotionSubspaceVector";
+  case 950: return "FixedJoint_computeChildPosVelAcc";
+  case 951: return "FixedJoint_computeChildVelAcc";
+  case 952: return "FixedJoint_computeChildVel";
+  case 953: return "FixedJoint_computeChildAcc";
+  case 954: return "FixedJoint_computeChildBiasAcc";
+  case 955: return "FixedJoint_computeJointTorque";
+  case 956: return "FixedJoint_setIndex";
+  case 957: return "FixedJoint_getIndex";
+  case 958: return "FixedJoint_setPosCoordsOffset";
+  case 959: return "FixedJoint_getPosCoordsOffset";
+  case 960: return "FixedJoint_setDOFsOffset";
+  case 961: return "FixedJoint_getDOFsOffset";
+  case 962: return "FixedJoint_hasPosLimits";
+  case 963: return "FixedJoint_enablePosLimits";
+  case 964: return "FixedJoint_getPosLimits";
+  case 965: return "FixedJoint_getMinPosLimit";
+  case 966: return "FixedJoint_getMaxPosLimit";
+  case 967: return "FixedJoint_setPosLimits";
+  case 968: return "delete_MovableJointImpl1";
+  case 969: return "MovableJointImpl1_getNrOfPosCoords";
+  case 970: return "MovableJointImpl1_getNrOfDOFs";
+  case 971: return "MovableJointImpl1_setIndex";
+  case 972: return "MovableJointImpl1_getIndex";
+  case 973: return "MovableJointImpl1_setPosCoordsOffset";
+  case 974: return "MovableJointImpl1_getPosCoordsOffset";
+  case 975: return "MovableJointImpl1_setDOFsOffset";
+  case 976: return "MovableJointImpl1_getDOFsOffset";
+  case 977: return "delete_MovableJointImpl2";
+  case 978: return "MovableJointImpl2_getNrOfPosCoords";
+  case 979: return "MovableJointImpl2_getNrOfDOFs";
+  case 980: return "MovableJointImpl2_setIndex";
+  case 981: return "MovableJointImpl2_getIndex";
+  case 982: return "MovableJointImpl2_setPosCoordsOffset";
+  case 983: return "MovableJointImpl2_getPosCoordsOffset";
+  case 984: return "MovableJointImpl2_setDOFsOffset";
+  case 985: return "MovableJointImpl2_getDOFsOffset";
+  case 986: return "delete_MovableJointImpl3";
+  case 987: return "MovableJointImpl3_getNrOfPosCoords";
+  case 988: return "MovableJointImpl3_getNrOfDOFs";
+  case 989: return "MovableJointImpl3_setIndex";
+  case 990: return "MovableJointImpl3_getIndex";
+  case 991: return "MovableJointImpl3_setPosCoordsOffset";
+  case 992: return "MovableJointImpl3_getPosCoordsOffset";
+  case 993: return "MovableJointImpl3_setDOFsOffset";
+  case 994: return "MovableJointImpl3_getDOFsOffset";
+  case 995: return "delete_MovableJointImpl4";
+  case 996: return "MovableJointImpl4_getNrOfPosCoords";
+  case 997: return "MovableJointImpl4_getNrOfDOFs";
+  case 998: return "MovableJointImpl4_setIndex";
+  case 999: return "MovableJointImpl4_getIndex";
+  case 1000: return "MovableJointImpl4_setPosCoordsOffset";
+  case 1001: return "MovableJointImpl4_getPosCoordsOffset";
+  case 1002: return "MovableJointImpl4_setDOFsOffset";
+  case 1003: return "MovableJointImpl4_getDOFsOffset";
+  case 1004: return "delete_MovableJointImpl5";
+  case 1005: return "MovableJointImpl5_getNrOfPosCoords";
+  case 1006: return "MovableJointImpl5_getNrOfDOFs";
+  case 1007: return "MovableJointImpl5_setIndex";
+  case 1008: return "MovableJointImpl5_getIndex";
+  case 1009: return "MovableJointImpl5_setPosCoordsOffset";
+  case 1010: return "MovableJointImpl5_getPosCoordsOffset";
+  case 1011: return "MovableJointImpl5_setDOFsOffset";
+  case 1012: return "MovableJointImpl5_getDOFsOffset";
+  case 1013: return "delete_MovableJointImpl6";
+  case 1014: return "MovableJointImpl6_getNrOfPosCoords";
+  case 1015: return "MovableJointImpl6_getNrOfDOFs";
+  case 1016: return "MovableJointImpl6_setIndex";
+  case 1017: return "MovableJointImpl6_getIndex";
+  case 1018: return "MovableJointImpl6_setPosCoordsOffset";
+  case 1019: return "MovableJointImpl6_getPosCoordsOffset";
+  case 1020: return "MovableJointImpl6_setDOFsOffset";
+  case 1021: return "MovableJointImpl6_getDOFsOffset";
+  case 1022: return "new_RevoluteJoint";
+  case 1023: return "delete_RevoluteJoint";
+  case 1024: return "RevoluteJoint_clone";
+  case 1025: return "RevoluteJoint_setAttachedLinks";
+  case 1026: return "RevoluteJoint_setRestTransform";
+  case 1027: return "RevoluteJoint_setAxis";
+  case 1028: return "RevoluteJoint_getFirstAttachedLink";
+  case 1029: return "RevoluteJoint_getSecondAttachedLink";
+  case 1030: return "RevoluteJoint_getAxis";
+  case 1031: return "RevoluteJoint_getRestTransform";
+  case 1032: return "RevoluteJoint_getTransform";
+  case 1033: return "RevoluteJoint_getTransformDerivative";
+  case 1034: return "RevoluteJoint_getMotionSubspaceVector";
+  case 1035: return "RevoluteJoint_computeChildPosVelAcc";
+  case 1036: return "RevoluteJoint_computeChildVel";
+  case 1037: return "RevoluteJoint_computeChildVelAcc";
+  case 1038: return "RevoluteJoint_computeChildAcc";
+  case 1039: return "RevoluteJoint_computeChildBiasAcc";
+  case 1040: return "RevoluteJoint_computeJointTorque";
+  case 1041: return "RevoluteJoint_hasPosLimits";
+  case 1042: return "RevoluteJoint_enablePosLimits";
+  case 1043: return "RevoluteJoint_getPosLimits";
+  case 1044: return "RevoluteJoint_getMinPosLimit";
+  case 1045: return "RevoluteJoint_getMaxPosLimit";
+  case 1046: return "RevoluteJoint_setPosLimits";
+  case 1047: return "new_PrismaticJoint";
+  case 1048: return "delete_PrismaticJoint";
+  case 1049: return "PrismaticJoint_clone";
+  case 1050: return "PrismaticJoint_setAttachedLinks";
+  case 1051: return "PrismaticJoint_setRestTransform";
+  case 1052: return "PrismaticJoint_setAxis";
+  case 1053: return "PrismaticJoint_getFirstAttachedLink";
+  case 1054: return "PrismaticJoint_getSecondAttachedLink";
+  case 1055: return "PrismaticJoint_getAxis";
+  case 1056: return "PrismaticJoint_getRestTransform";
+  case 1057: return "PrismaticJoint_getTransform";
+  case 1058: return "PrismaticJoint_getTransformDerivative";
+  case 1059: return "PrismaticJoint_getMotionSubspaceVector";
+  case 1060: return "PrismaticJoint_computeChildPosVelAcc";
+  case 1061: return "PrismaticJoint_computeChildVel";
+  case 1062: return "PrismaticJoint_computeChildVelAcc";
+  case 1063: return "PrismaticJoint_computeChildAcc";
+  case 1064: return "PrismaticJoint_computeChildBiasAcc";
+  case 1065: return "PrismaticJoint_computeJointTorque";
+  case 1066: return "PrismaticJoint_hasPosLimits";
+  case 1067: return "PrismaticJoint_enablePosLimits";
+  case 1068: return "PrismaticJoint_getPosLimits";
+  case 1069: return "PrismaticJoint_getMinPosLimit";
+  case 1070: return "PrismaticJoint_getMaxPosLimit";
+  case 1071: return "PrismaticJoint_setPosLimits";
+  case 1072: return "new_Traversal";
+  case 1073: return "delete_Traversal";
+  case 1074: return "Traversal_getNrOfVisitedLinks";
+  case 1075: return "Traversal_getLink";
+  case 1076: return "Traversal_getBaseLink";
+  case 1077: return "Traversal_getParentLink";
+  case 1078: return "Traversal_getParentJoint";
+  case 1079: return "Traversal_getParentLinkFromLinkIndex";
+  case 1080: return "Traversal_getParentJointFromLinkIndex";
+  case 1081: return "Traversal_getTraversalIndexFromLinkIndex";
+  case 1082: return "Traversal_reset";
+  case 1083: return "Traversal_addTraversalBase";
+  case 1084: return "Traversal_addTraversalElement";
+  case 1085: return "Traversal_isParentOf";
+  case 1086: return "Traversal_getChildLinkIndexFromJointIndex";
+  case 1087: return "Traversal_getParentLinkIndexFromJointIndex";
+  case 1088: return "Traversal_toString";
+  case 1089: return "delete_SolidShape";
+  case 1090: return "SolidShape_clone";
+  case 1091: return "SolidShape_name_get";
+  case 1092: return "SolidShape_name_set";
+  case 1093: return "SolidShape_link_H_geometry_get";
+  case 1094: return "SolidShape_link_H_geometry_set";
+  case 1095: return "SolidShape_material_get";
+  case 1096: return "SolidShape_material_set";
+  case 1097: return "SolidShape_isSphere";
+  case 1098: return "SolidShape_isBox";
+  case 1099: return "SolidShape_isCylinder";
+  case 1100: return "SolidShape_isExternalMesh";
+  case 1101: return "SolidShape_asSphere";
+  case 1102: return "SolidShape_asBox";
+  case 1103: return "SolidShape_asCylinder";
+  case 1104: return "SolidShape_asExternalMesh";
+  case 1105: return "delete_Sphere";
+  case 1106: return "Sphere_clone";
+  case 1107: return "Sphere_radius_get";
+  case 1108: return "Sphere_radius_set";
+  case 1109: return "new_Sphere";
+  case 1110: return "delete_Box";
+  case 1111: return "Box_clone";
+  case 1112: return "Box_x_get";
+  case 1113: return "Box_x_set";
+  case 1114: return "Box_y_get";
+  case 1115: return "Box_y_set";
+  case 1116: return "Box_z_get";
+  case 1117: return "Box_z_set";
+  case 1118: return "new_Box";
+  case 1119: return "delete_Cylinder";
+  case 1120: return "Cylinder_clone";
+  case 1121: return "Cylinder_length_get";
+  case 1122: return "Cylinder_length_set";
+  case 1123: return "Cylinder_radius_get";
+  case 1124: return "Cylinder_radius_set";
+  case 1125: return "new_Cylinder";
+  case 1126: return "delete_ExternalMesh";
+  case 1127: return "ExternalMesh_clone";
+  case 1128: return "ExternalMesh_filename_get";
+  case 1129: return "ExternalMesh_filename_set";
+  case 1130: return "ExternalMesh_scale_get";
+  case 1131: return "ExternalMesh_scale_set";
+  case 1132: return "new_ExternalMesh";
+  case 1133: return "new_ModelSolidShapes";
+  case 1134: return "ModelSolidShapes_clear";
+  case 1135: return "delete_ModelSolidShapes";
+  case 1136: return "ModelSolidShapes_resize";
+  case 1137: return "ModelSolidShapes_isConsistent";
+  case 1138: return "ModelSolidShapes_linkSolidShapes_get";
+  case 1139: return "ModelSolidShapes_linkSolidShapes_set";
+  case 1140: return "Neighbor_neighborLink_get";
+  case 1141: return "Neighbor_neighborLink_set";
+  case 1142: return "Neighbor_neighborJoint_get";
+  case 1143: return "Neighbor_neighborJoint_set";
+  case 1144: return "new_Neighbor";
+  case 1145: return "delete_Neighbor";
+  case 1146: return "new_Model";
+  case 1147: return "Model_copy";
+  case 1148: return "delete_Model";
+  case 1149: return "Model_getNrOfLinks";
+  case 1150: return "Model_getLinkName";
+  case 1151: return "Model_getLinkIndex";
+  case 1152: return "Model_isValidLinkIndex";
+  case 1153: return "Model_getLink";
+  case 1154: return "Model_addLink";
+  case 1155: return "Model_getNrOfJoints";
+  case 1156: return "Model_getJointName";
+  case 1157: return "Model_getJointIndex";
+  case 1158: return "Model_getJoint";
+  case 1159: return "Model_isValidJointIndex";
+  case 1160: return "Model_isLinkNameUsed";
+  case 1161: return "Model_isJointNameUsed";
+  case 1162: return "Model_isFrameNameUsed";
+  case 1163: return "Model_addJoint";
+  case 1164: return "Model_addJointAndLink";
+  case 1165: return "Model_insertLinkToExistingJointAndAddJointForDisplacedLink";
+  case 1166: return "Model_getNrOfPosCoords";
+  case 1167: return "Model_getNrOfDOFs";
+  case 1168: return "Model_getNrOfFrames";
+  case 1169: return "Model_addAdditionalFrameToLink";
+  case 1170: return "Model_getFrameName";
+  case 1171: return "Model_getFrameIndex";
+  case 1172: return "Model_isValidFrameIndex";
+  case 1173: return "Model_getFrameTransform";
+  case 1174: return "Model_getFrameLink";
+  case 1175: return "Model_getNrOfNeighbors";
+  case 1176: return "Model_getNeighbor";
+  case 1177: return "Model_setDefaultBaseLink";
+  case 1178: return "Model_getDefaultBaseLink";
+  case 1179: return "Model_computeFullTreeTraversal";
+  case 1180: return "Model_getInertialParameters";
+  case 1181: return "Model_updateInertialParameters";
+  case 1182: return "Model_visualSolidShapes";
+  case 1183: return "Model_collisionSolidShapes";
+  case 1184: return "Model_toString";
+  case 1185: return "new_JointPosDoubleArray";
+  case 1186: return "JointPosDoubleArray_resize";
+  case 1187: return "JointPosDoubleArray_isConsistent";
+  case 1188: return "delete_JointPosDoubleArray";
+  case 1189: return "new_JointDOFsDoubleArray";
+  case 1190: return "JointDOFsDoubleArray_resize";
+  case 1191: return "JointDOFsDoubleArray_isConsistent";
+  case 1192: return "delete_JointDOFsDoubleArray";
+  case 1193: return "new_DOFSpatialForceArray";
+  case 1194: return "DOFSpatialForceArray_resize";
+  case 1195: return "DOFSpatialForceArray_isConsistent";
+  case 1196: return "DOFSpatialForceArray_paren";
+  case 1197: return "delete_DOFSpatialForceArray";
+  case 1198: return "new_DOFSpatialMotionArray";
+  case 1199: return "DOFSpatialMotionArray_resize";
+  case 1200: return "DOFSpatialMotionArray_isConsistent";
+  case 1201: return "DOFSpatialMotionArray_paren";
+  case 1202: return "delete_DOFSpatialMotionArray";
+  case 1203: return "new_FrameFreeFloatingJacobian";
+  case 1204: return "FrameFreeFloatingJacobian_resize";
+  case 1205: return "FrameFreeFloatingJacobian_isConsistent";
+  case 1206: return "delete_FrameFreeFloatingJacobian";
+  case 1207: return "new_MomentumFreeFloatingJacobian";
+  case 1208: return "MomentumFreeFloatingJacobian_resize";
+  case 1209: return "MomentumFreeFloatingJacobian_isConsistent";
+  case 1210: return "delete_MomentumFreeFloatingJacobian";
+  case 1211: return "new_FreeFloatingMassMatrix";
+  case 1212: return "FreeFloatingMassMatrix_resize";
+  case 1213: return "delete_FreeFloatingMassMatrix";
+  case 1214: return "new_FreeFloatingPos";
+  case 1215: return "FreeFloatingPos_resize";
+  case 1216: return "FreeFloatingPos_worldBasePos";
+  case 1217: return "FreeFloatingPos_jointPos";
+  case 1218: return "FreeFloatingPos_getNrOfPosCoords";
+  case 1219: return "delete_FreeFloatingPos";
+  case 1220: return "new_FreeFloatingGeneralizedTorques";
+  case 1221: return "FreeFloatingGeneralizedTorques_resize";
+  case 1222: return "FreeFloatingGeneralizedTorques_baseWrench";
+  case 1223: return "FreeFloatingGeneralizedTorques_jointTorques";
+  case 1224: return "FreeFloatingGeneralizedTorques_getNrOfDOFs";
+  case 1225: return "delete_FreeFloatingGeneralizedTorques";
+  case 1226: return "new_FreeFloatingVel";
+  case 1227: return "FreeFloatingVel_resize";
+  case 1228: return "FreeFloatingVel_baseVel";
+  case 1229: return "FreeFloatingVel_jointVel";
+  case 1230: return "FreeFloatingVel_getNrOfDOFs";
+  case 1231: return "delete_FreeFloatingVel";
+  case 1232: return "new_FreeFloatingAcc";
+  case 1233: return "FreeFloatingAcc_resize";
+  case 1234: return "FreeFloatingAcc_baseAcc";
+  case 1235: return "FreeFloatingAcc_jointAcc";
+  case 1236: return "FreeFloatingAcc_getNrOfDOFs";
+  case 1237: return "delete_FreeFloatingAcc";
+  case 1238: return "ContactWrench_contactId";
+  case 1239: return "ContactWrench_contactPoint";
+  case 1240: return "ContactWrench_contactWrench";
+  case 1241: return "new_ContactWrench";
+  case 1242: return "delete_ContactWrench";
+  case 1243: return "new_LinkContactWrenches";
+  case 1244: return "LinkContactWrenches_resize";
+  case 1245: return "LinkContactWrenches_getNrOfContactsForLink";
+  case 1246: return "LinkContactWrenches_setNrOfContactsForLink";
+  case 1247: return "LinkContactWrenches_getNrOfLinks";
+  case 1248: return "LinkContactWrenches_contactWrench";
+  case 1249: return "LinkContactWrenches_computeNetWrenches";
+  case 1250: return "LinkContactWrenches_toString";
+  case 1251: return "delete_LinkContactWrenches";
+  case 1252: return "_wrap_ForwardPositionKinematics";
+  case 1253: return "_wrap_ForwardVelAccKinematics";
+  case 1254: return "_wrap_ForwardPosVelAccKinematics";
+  case 1255: return "_wrap_ForwardPosVelKinematics";
+  case 1256: return "_wrap_ForwardAccKinematics";
+  case 1257: return "_wrap_ForwardBiasAccKinematics";
+  case 1258: return "_wrap_ComputeLinearAndAngularMomentum";
+  case 1259: return "_wrap_ComputeLinearAndAngularMomentumDerivativeBias";
+  case 1260: return "_wrap_RNEADynamicPhase";
+  case 1261: return "_wrap_CompositeRigidBodyAlgorithm";
+  case 1262: return "new_ArticulatedBodyAlgorithmInternalBuffers";
+  case 1263: return "ArticulatedBodyAlgorithmInternalBuffers_resize";
+  case 1264: return "ArticulatedBodyAlgorithmInternalBuffers_isConsistent";
+  case 1265: return "ArticulatedBodyAlgorithmInternalBuffers_S_get";
+  case 1266: return "ArticulatedBodyAlgorithmInternalBuffers_S_set";
+  case 1267: return "ArticulatedBodyAlgorithmInternalBuffers_U_get";
+  case 1268: return "ArticulatedBodyAlgorithmInternalBuffers_U_set";
+  case 1269: return "ArticulatedBodyAlgorithmInternalBuffers_D_get";
+  case 1270: return "ArticulatedBodyAlgorithmInternalBuffers_D_set";
+  case 1271: return "ArticulatedBodyAlgorithmInternalBuffers_u_get";
+  case 1272: return "ArticulatedBodyAlgorithmInternalBuffers_u_set";
+  case 1273: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_get";
+  case 1274: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set";
+  case 1275: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get";
+  case 1276: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set";
+  case 1277: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get";
+  case 1278: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set";
+  case 1279: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get";
+  case 1280: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set";
+  case 1281: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get";
+  case 1282: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set";
+  case 1283: return "delete_ArticulatedBodyAlgorithmInternalBuffers";
+  case 1284: return "_wrap_ArticulatedBodyAlgorithm";
+  case 1285: return "_wrap_InverseDynamicsInertialParametersRegressor";
+  case 1286: return "NR_OF_SENSOR_TYPES_get";
+  case 1287: return "_wrap_isLinkSensor";
+  case 1288: return "_wrap_isJointSensor";
+  case 1289: return "_wrap_getSensorTypeSize";
+  case 1290: return "delete_Sensor";
+  case 1291: return "Sensor_getName";
+  case 1292: return "Sensor_getSensorType";
+  case 1293: return "Sensor_isValid";
+  case 1294: return "Sensor_setName";
+  case 1295: return "Sensor_clone";
+  case 1296: return "Sensor_isConsistent";
+  case 1297: return "Sensor_updateIndices";
+  case 1298: return "Sensor_updateIndeces";
+  case 1299: return "delete_JointSensor";
+  case 1300: return "JointSensor_getParentJoint";
+  case 1301: return "JointSensor_getParentJointIndex";
+  case 1302: return "JointSensor_setParentJoint";
+  case 1303: return "JointSensor_setParentJointIndex";
+  case 1304: return "JointSensor_isConsistent";
+  case 1305: return "delete_LinkSensor";
+  case 1306: return "LinkSensor_getParentLink";
+  case 1307: return "LinkSensor_getParentLinkIndex";
+  case 1308: return "LinkSensor_getLinkSensorTransform";
+  case 1309: return "LinkSensor_setParentLink";
+  case 1310: return "LinkSensor_setParentLinkIndex";
+  case 1311: return "LinkSensor_setLinkSensorTransform";
+  case 1312: return "LinkSensor_isConsistent";
+  case 1313: return "new_SensorsList";
+  case 1314: return "delete_SensorsList";
+  case 1315: return "SensorsList_addSensor";
+  case 1316: return "SensorsList_setSerialization";
+  case 1317: return "SensorsList_getSerialization";
+  case 1318: return "SensorsList_getNrOfSensors";
+  case 1319: return "SensorsList_getSensorIndex";
+  case 1320: return "SensorsList_getSizeOfAllSensorsMeasurements";
+  case 1321: return "SensorsList_getSensor";
+  case 1322: return "SensorsList_isConsistent";
+  case 1323: return "SensorsList_removeSensor";
+  case 1324: return "SensorsList_removeAllSensorsOfType";
+  case 1325: return "SensorsList_getSixAxisForceTorqueSensor";
+  case 1326: return "SensorsList_getAccelerometerSensor";
+  case 1327: return "SensorsList_getGyroscopeSensor";
+  case 1328: return "SensorsList_getThreeAxisAngularAccelerometerSensor";
+  case 1329: return "SensorsList_getThreeAxisForceTorqueContactSensor";
+  case 1330: return "new_SensorsMeasurements";
+  case 1331: return "delete_SensorsMeasurements";
+  case 1332: return "SensorsMeasurements_setNrOfSensors";
+  case 1333: return "SensorsMeasurements_getNrOfSensors";
+  case 1334: return "SensorsMeasurements_resize";
+  case 1335: return "SensorsMeasurements_toVector";
+  case 1336: return "SensorsMeasurements_setMeasurement";
+  case 1337: return "SensorsMeasurements_getMeasurement";
+  case 1338: return "SensorsMeasurements_getSizeOfAllSensorsMeasurements";
+  case 1339: return "new_SixAxisForceTorqueSensor";
+  case 1340: return "delete_SixAxisForceTorqueSensor";
+  case 1341: return "SixAxisForceTorqueSensor_setName";
+  case 1342: return "SixAxisForceTorqueSensor_setFirstLinkSensorTransform";
+  case 1343: return "SixAxisForceTorqueSensor_setSecondLinkSensorTransform";
+  case 1344: return "SixAxisForceTorqueSensor_getFirstLinkIndex";
+  case 1345: return "SixAxisForceTorqueSensor_getSecondLinkIndex";
+  case 1346: return "SixAxisForceTorqueSensor_setFirstLinkName";
+  case 1347: return "SixAxisForceTorqueSensor_setSecondLinkName";
+  case 1348: return "SixAxisForceTorqueSensor_getFirstLinkName";
+  case 1349: return "SixAxisForceTorqueSensor_getSecondLinkName";
+  case 1350: return "SixAxisForceTorqueSensor_setParentJoint";
+  case 1351: return "SixAxisForceTorqueSensor_setParentJointIndex";
+  case 1352: return "SixAxisForceTorqueSensor_setAppliedWrenchLink";
+  case 1353: return "SixAxisForceTorqueSensor_getName";
+  case 1354: return "SixAxisForceTorqueSensor_getSensorType";
+  case 1355: return "SixAxisForceTorqueSensor_getParentJoint";
+  case 1356: return "SixAxisForceTorqueSensor_getParentJointIndex";
+  case 1357: return "SixAxisForceTorqueSensor_isValid";
+  case 1358: return "SixAxisForceTorqueSensor_clone";
+  case 1359: return "SixAxisForceTorqueSensor_updateIndices";
+  case 1360: return "SixAxisForceTorqueSensor_updateIndeces";
+  case 1361: return "SixAxisForceTorqueSensor_getAppliedWrenchLink";
+  case 1362: return "SixAxisForceTorqueSensor_isLinkAttachedToSensor";
+  case 1363: return "SixAxisForceTorqueSensor_getLinkSensorTransform";
+  case 1364: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLink";
+  case 1365: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix";
+  case 1366: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix";
+  case 1367: return "SixAxisForceTorqueSensor_predictMeasurement";
+  case 1368: return "SixAxisForceTorqueSensor_toString";
+  case 1369: return "new_AccelerometerSensor";
+  case 1370: return "delete_AccelerometerSensor";
+  case 1371: return "AccelerometerSensor_setName";
+  case 1372: return "AccelerometerSensor_setLinkSensorTransform";
+  case 1373: return "AccelerometerSensor_setParentLink";
+  case 1374: return "AccelerometerSensor_setParentLinkIndex";
+  case 1375: return "AccelerometerSensor_getName";
+  case 1376: return "AccelerometerSensor_getSensorType";
+  case 1377: return "AccelerometerSensor_getParentLink";
+  case 1378: return "AccelerometerSensor_getParentLinkIndex";
+  case 1379: return "AccelerometerSensor_getLinkSensorTransform";
+  case 1380: return "AccelerometerSensor_isValid";
+  case 1381: return "AccelerometerSensor_clone";
+  case 1382: return "AccelerometerSensor_updateIndices";
+  case 1383: return "AccelerometerSensor_updateIndeces";
+  case 1384: return "AccelerometerSensor_predictMeasurement";
+  case 1385: return "new_GyroscopeSensor";
+  case 1386: return "delete_GyroscopeSensor";
+  case 1387: return "GyroscopeSensor_setName";
+  case 1388: return "GyroscopeSensor_setLinkSensorTransform";
+  case 1389: return "GyroscopeSensor_setParentLink";
+  case 1390: return "GyroscopeSensor_setParentLinkIndex";
+  case 1391: return "GyroscopeSensor_getName";
+  case 1392: return "GyroscopeSensor_getSensorType";
+  case 1393: return "GyroscopeSensor_getParentLink";
+  case 1394: return "GyroscopeSensor_getParentLinkIndex";
+  case 1395: return "GyroscopeSensor_getLinkSensorTransform";
+  case 1396: return "GyroscopeSensor_isValid";
+  case 1397: return "GyroscopeSensor_clone";
+  case 1398: return "GyroscopeSensor_updateIndices";
+  case 1399: return "GyroscopeSensor_updateIndeces";
+  case 1400: return "GyroscopeSensor_predictMeasurement";
+  case 1401: return "new_ThreeAxisAngularAccelerometerSensor";
+  case 1402: return "delete_ThreeAxisAngularAccelerometerSensor";
+  case 1403: return "ThreeAxisAngularAccelerometerSensor_setName";
+  case 1404: return "ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform";
+  case 1405: return "ThreeAxisAngularAccelerometerSensor_setParentLink";
+  case 1406: return "ThreeAxisAngularAccelerometerSensor_setParentLinkIndex";
+  case 1407: return "ThreeAxisAngularAccelerometerSensor_getName";
+  case 1408: return "ThreeAxisAngularAccelerometerSensor_getSensorType";
+  case 1409: return "ThreeAxisAngularAccelerometerSensor_getParentLink";
+  case 1410: return "ThreeAxisAngularAccelerometerSensor_getParentLinkIndex";
+  case 1411: return "ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform";
+  case 1412: return "ThreeAxisAngularAccelerometerSensor_isValid";
+  case 1413: return "ThreeAxisAngularAccelerometerSensor_clone";
+  case 1414: return "ThreeAxisAngularAccelerometerSensor_updateIndices";
+  case 1415: return "ThreeAxisAngularAccelerometerSensor_predictMeasurement";
+  case 1416: return "new_ThreeAxisForceTorqueContactSensor";
+  case 1417: return "delete_ThreeAxisForceTorqueContactSensor";
+  case 1418: return "ThreeAxisForceTorqueContactSensor_setName";
+  case 1419: return "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform";
+  case 1420: return "ThreeAxisForceTorqueContactSensor_setParentLink";
+  case 1421: return "ThreeAxisForceTorqueContactSensor_setParentLinkIndex";
+  case 1422: return "ThreeAxisForceTorqueContactSensor_getName";
+  case 1423: return "ThreeAxisForceTorqueContactSensor_getSensorType";
+  case 1424: return "ThreeAxisForceTorqueContactSensor_getParentLink";
+  case 1425: return "ThreeAxisForceTorqueContactSensor_getParentLinkIndex";
+  case 1426: return "ThreeAxisForceTorqueContactSensor_getLinkSensorTransform";
+  case 1427: return "ThreeAxisForceTorqueContactSensor_isValid";
+  case 1428: return "ThreeAxisForceTorqueContactSensor_clone";
+  case 1429: return "ThreeAxisForceTorqueContactSensor_updateIndices";
+  case 1430: return "ThreeAxisForceTorqueContactSensor_setLoadCellLocations";
+  case 1431: return "ThreeAxisForceTorqueContactSensor_getLoadCellLocations";
+  case 1432: return "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements";
+  case 1433: return "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements";
+  case 1434: return "_wrap_predictSensorsMeasurements";
+  case 1435: return "_wrap_predictSensorsMeasurementsFromRawBuffers";
+  case 1436: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_get";
+  case 1437: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_set";
+  case 1438: return "URDFParserOptions_originalFilename_get";
+  case 1439: return "URDFParserOptions_originalFilename_set";
+  case 1440: return "new_URDFParserOptions";
+  case 1441: return "delete_URDFParserOptions";
+  case 1442: return "_wrap_modelFromURDF";
+  case 1443: return "_wrap_modelFromURDFString";
+  case 1444: return "_wrap_dofsListFromURDF";
+  case 1445: return "_wrap_dofsListFromURDFString";
+  case 1446: return "_wrap_sensorsFromURDF";
+  case 1447: return "_wrap_sensorsFromURDFString";
+  case 1448: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_get";
+  case 1449: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_set";
+  case 1450: return "ModelParserOptions_originalFilename_get";
+  case 1451: return "ModelParserOptions_originalFilename_set";
+  case 1452: return "new_ModelParserOptions";
+  case 1453: return "delete_ModelParserOptions";
+  case 1454: return "new_ModelLoader";
+  case 1455: return "delete_ModelLoader";
+  case 1456: return "ModelLoader_parsingOptions";
+  case 1457: return "ModelLoader_setParsingOptions";
+  case 1458: return "ModelLoader_loadModelFromString";
+  case 1459: return "ModelLoader_loadModelFromFile";
+  case 1460: return "ModelLoader_loadReducedModelFromFullModel";
+  case 1461: return "ModelLoader_loadReducedModelFromString";
+  case 1462: return "ModelLoader_loadReducedModelFromFile";
+  case 1463: return "ModelLoader_model";
+  case 1464: return "ModelLoader_sensors";
+  case 1465: return "ModelLoader_isValid";
+  case 1466: return "new_UnknownWrenchContact";
+  case 1467: return "UnknownWrenchContact_unknownType_get";
+  case 1468: return "UnknownWrenchContact_unknownType_set";
+  case 1469: return "UnknownWrenchContact_contactPoint_get";
+  case 1470: return "UnknownWrenchContact_contactPoint_set";
+  case 1471: return "UnknownWrenchContact_forceDirection_get";
+  case 1472: return "UnknownWrenchContact_forceDirection_set";
+  case 1473: return "UnknownWrenchContact_knownWrench_get";
+  case 1474: return "UnknownWrenchContact_knownWrench_set";
+  case 1475: return "UnknownWrenchContact_contactId_get";
+  case 1476: return "UnknownWrenchContact_contactId_set";
+  case 1477: return "delete_UnknownWrenchContact";
+  case 1478: return "new_LinkUnknownWrenchContacts";
+  case 1479: return "LinkUnknownWrenchContacts_clear";
+  case 1480: return "LinkUnknownWrenchContacts_resize";
+  case 1481: return "LinkUnknownWrenchContacts_getNrOfContactsForLink";
+  case 1482: return "LinkUnknownWrenchContacts_setNrOfContactsForLink";
+  case 1483: return "LinkUnknownWrenchContacts_addNewContactForLink";
+  case 1484: return "LinkUnknownWrenchContacts_addNewContactInFrame";
+  case 1485: return "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin";
+  case 1486: return "LinkUnknownWrenchContacts_contactWrench";
+  case 1487: return "LinkUnknownWrenchContacts_toString";
+  case 1488: return "delete_LinkUnknownWrenchContacts";
+  case 1489: return "new_estimateExternalWrenchesBuffers";
+  case 1490: return "estimateExternalWrenchesBuffers_resize";
+  case 1491: return "estimateExternalWrenchesBuffers_getNrOfSubModels";
+  case 1492: return "estimateExternalWrenchesBuffers_getNrOfLinks";
+  case 1493: return "estimateExternalWrenchesBuffers_isConsistent";
+  case 1494: return "estimateExternalWrenchesBuffers_A_get";
+  case 1495: return "estimateExternalWrenchesBuffers_A_set";
+  case 1496: return "estimateExternalWrenchesBuffers_x_get";
+  case 1497: return "estimateExternalWrenchesBuffers_x_set";
+  case 1498: return "estimateExternalWrenchesBuffers_b_get";
+  case 1499: return "estimateExternalWrenchesBuffers_b_set";
+  case 1500: return "estimateExternalWrenchesBuffers_pinvA_get";
+  case 1501: return "estimateExternalWrenchesBuffers_pinvA_set";
+  case 1502: return "estimateExternalWrenchesBuffers_b_contacts_subtree_get";
+  case 1503: return "estimateExternalWrenchesBuffers_b_contacts_subtree_set";
+  case 1504: return "estimateExternalWrenchesBuffers_subModelBase_H_link_get";
+  case 1505: return "estimateExternalWrenchesBuffers_subModelBase_H_link_set";
+  case 1506: return "delete_estimateExternalWrenchesBuffers";
+  case 1507: return "_wrap_estimateExternalWrenchesWithoutInternalFT";
+  case 1508: return "_wrap_estimateExternalWrenches";
+  case 1509: return "_wrap_dynamicsEstimationForwardVelAccKinematics";
+  case 1510: return "_wrap_dynamicsEstimationForwardVelKinematics";
+  case 1511: return "_wrap_computeLinkNetWrenchesWithoutGravity";
+  case 1512: return "_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches";
+  case 1513: return "new_ExtWrenchesAndJointTorquesEstimator";
+  case 1514: return "delete_ExtWrenchesAndJointTorquesEstimator";
+  case 1515: return "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors";
+  case 1516: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile";
+  case 1517: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs";
+  case 1518: return "ExtWrenchesAndJointTorquesEstimator_model";
+  case 1519: return "ExtWrenchesAndJointTorquesEstimator_sensors";
+  case 1520: return "ExtWrenchesAndJointTorquesEstimator_submodels";
+  case 1521: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase";
+  case 1522: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase";
+  case 1523: return "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements";
+  case 1524: return "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques";
+  case 1525: return "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill";
+  case 1526: return "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity";
+  case 1527: return "new_SimpleLeggedOdometry";
+  case 1528: return "delete_SimpleLeggedOdometry";
+  case 1529: return "SimpleLeggedOdometry_setModel";
+  case 1530: return "SimpleLeggedOdometry_loadModelFromFile";
+  case 1531: return "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs";
+  case 1532: return "SimpleLeggedOdometry_model";
+  case 1533: return "SimpleLeggedOdometry_updateKinematics";
+  case 1534: return "SimpleLeggedOdometry_init";
+  case 1535: return "SimpleLeggedOdometry_changeFixedFrame";
+  case 1536: return "SimpleLeggedOdometry_getCurrentFixedLink";
+  case 1537: return "SimpleLeggedOdometry_getWorldLinkTransform";
+  case 1538: return "_wrap_isLinkBerdyDynamicVariable";
+  case 1539: return "_wrap_isJointBerdyDynamicVariable";
+  case 1540: return "_wrap_isDOFBerdyDynamicVariable";
+  case 1541: return "new_BerdyOptions";
+  case 1542: return "BerdyOptions_berdyVariant_get";
+  case 1543: return "BerdyOptions_berdyVariant_set";
+  case 1544: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get";
+  case 1545: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set";
+  case 1546: return "BerdyOptions_includeAllJointAccelerationsAsSensors_get";
+  case 1547: return "BerdyOptions_includeAllJointAccelerationsAsSensors_set";
+  case 1548: return "BerdyOptions_includeAllJointTorquesAsSensors_get";
+  case 1549: return "BerdyOptions_includeAllJointTorquesAsSensors_set";
+  case 1550: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get";
+  case 1551: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set";
+  case 1552: return "BerdyOptions_includeFixedBaseExternalWrench_get";
+  case 1553: return "BerdyOptions_includeFixedBaseExternalWrench_set";
+  case 1554: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get";
+  case 1555: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set";
+  case 1556: return "BerdyOptions_baseLink_get";
+  case 1557: return "BerdyOptions_baseLink_set";
+  case 1558: return "BerdyOptions_checkConsistency";
+  case 1559: return "delete_BerdyOptions";
+  case 1560: return "BerdySensor_type_get";
+  case 1561: return "BerdySensor_type_set";
+  case 1562: return "BerdySensor_id_get";
+  case 1563: return "BerdySensor_id_set";
+  case 1564: return "BerdySensor_range_get";
+  case 1565: return "BerdySensor_range_set";
+  case 1566: return "BerdySensor_eq";
+  case 1567: return "BerdySensor_lt";
+  case 1568: return "new_BerdySensor";
+  case 1569: return "delete_BerdySensor";
+  case 1570: return "BerdyDynamicVariable_type_get";
+  case 1571: return "BerdyDynamicVariable_type_set";
+  case 1572: return "BerdyDynamicVariable_id_get";
+  case 1573: return "BerdyDynamicVariable_id_set";
+  case 1574: return "BerdyDynamicVariable_range_get";
+  case 1575: return "BerdyDynamicVariable_range_set";
+  case 1576: return "BerdyDynamicVariable_eq";
+  case 1577: return "BerdyDynamicVariable_lt";
+  case 1578: return "new_BerdyDynamicVariable";
+  case 1579: return "delete_BerdyDynamicVariable";
+  case 1580: return "new_BerdyHelper";
+  case 1581: return "BerdyHelper_dynamicTraversal";
+  case 1582: return "BerdyHelper_model";
+  case 1583: return "BerdyHelper_sensors";
+  case 1584: return "BerdyHelper_isValid";
+  case 1585: return "BerdyHelper_init";
+  case 1586: return "BerdyHelper_getOptions";
+  case 1587: return "BerdyHelper_getNrOfDynamicVariables";
+  case 1588: return "BerdyHelper_getNrOfDynamicEquations";
+  case 1589: return "BerdyHelper_getNrOfSensorsMeasurements";
+  case 1590: return "BerdyHelper_resizeAndZeroBerdyMatrices";
+  case 1591: return "BerdyHelper_getBerdyMatrices";
+  case 1592: return "BerdyHelper_getSensorsOrdering";
+  case 1593: return "BerdyHelper_getRangeSensorVariable";
+  case 1594: return "BerdyHelper_getRangeDOFSensorVariable";
+  case 1595: return "BerdyHelper_getRangeJointSensorVariable";
+  case 1596: return "BerdyHelper_getRangeLinkSensorVariable";
+  case 1597: return "BerdyHelper_getRangeLinkVariable";
+  case 1598: return "BerdyHelper_getRangeJointVariable";
+  case 1599: return "BerdyHelper_getRangeDOFVariable";
+  case 1600: return "BerdyHelper_getDynamicVariablesOrdering";
+  case 1601: return "BerdyHelper_serializeDynamicVariables";
+  case 1602: return "BerdyHelper_serializeSensorVariables";
+  case 1603: return "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA";
+  case 1604: return "BerdyHelper_extractJointTorquesFromDynamicVariables";
+  case 1605: return "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables";
+  case 1606: return "BerdyHelper_updateKinematicsFromFloatingBase";
+  case 1607: return "BerdyHelper_updateKinematicsFromFixedBase";
+  case 1608: return "BerdyHelper_updateKinematicsFromTraversalFixedBase";
+  case 1609: return "BerdyHelper_setNetExternalWrenchMeasurementFrame";
+  case 1610: return "BerdyHelper_getNetExternalWrenchMeasurementFrame";
+  case 1611: return "delete_BerdyHelper";
+  case 1612: return "new_BerdySparseMAPSolver";
+  case 1613: return "delete_BerdySparseMAPSolver";
+  case 1614: return "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance";
+  case 1615: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance";
+  case 1616: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue";
+  case 1617: return "BerdySparseMAPSolver_setMeasurementsPriorCovariance";
+  case 1618: return "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse";
+  case 1619: return "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse";
+  case 1620: return "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue";
+  case 1621: return "BerdySparseMAPSolver_measurementsPriorCovarianceInverse";
+  case 1622: return "BerdySparseMAPSolver_isValid";
+  case 1623: return "BerdySparseMAPSolver_initialize";
+  case 1624: return "BerdySparseMAPSolver_updateEstimateInformationFixedBase";
+  case 1625: return "BerdySparseMAPSolver_updateEstimateInformationFloatingBase";
+  case 1626: return "BerdySparseMAPSolver_doEstimate";
+  case 1627: return "BerdySparseMAPSolver_getLastEstimate";
+  case 1628: return "delete_IAttitudeEstimator";
+  case 1629: return "IAttitudeEstimator_updateFilterWithMeasurements";
+  case 1630: return "IAttitudeEstimator_propagateStates";
+  case 1631: return "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix";
+  case 1632: return "IAttitudeEstimator_getOrientationEstimateAsQuaternion";
+  case 1633: return "IAttitudeEstimator_getOrientationEstimateAsRPY";
+  case 1634: return "IAttitudeEstimator_getInternalStateSize";
+  case 1635: return "IAttitudeEstimator_getInternalState";
+  case 1636: return "IAttitudeEstimator_getDefaultInternalInitialState";
+  case 1637: return "IAttitudeEstimator_setInternalState";
+  case 1638: return "IAttitudeEstimator_setInternalStateInitialOrientation";
+  case 1639: return "AttitudeMahonyFilterParameters_time_step_in_seconds_get";
+  case 1640: return "AttitudeMahonyFilterParameters_time_step_in_seconds_set";
+  case 1641: return "AttitudeMahonyFilterParameters_kp_get";
+  case 1642: return "AttitudeMahonyFilterParameters_kp_set";
+  case 1643: return "AttitudeMahonyFilterParameters_ki_get";
+  case 1644: return "AttitudeMahonyFilterParameters_ki_set";
+  case 1645: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_get";
+  case 1646: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set";
+  case 1647: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get";
+  case 1648: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set";
+  case 1649: return "new_AttitudeMahonyFilterParameters";
+  case 1650: return "delete_AttitudeMahonyFilterParameters";
+  case 1651: return "new_AttitudeMahonyFilter";
+  case 1652: return "AttitudeMahonyFilter_useMagnetoMeterMeasurements";
+  case 1653: return "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements";
+  case 1654: return "AttitudeMahonyFilter_setGainkp";
+  case 1655: return "AttitudeMahonyFilter_setGainki";
+  case 1656: return "AttitudeMahonyFilter_setTimeStepInSeconds";
+  case 1657: return "AttitudeMahonyFilter_setGravityDirection";
+  case 1658: return "AttitudeMahonyFilter_setParameters";
+  case 1659: return "AttitudeMahonyFilter_getParameters";
+  case 1660: return "AttitudeMahonyFilter_updateFilterWithMeasurements";
+  case 1661: return "AttitudeMahonyFilter_propagateStates";
+  case 1662: return "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix";
+  case 1663: return "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion";
+  case 1664: return "AttitudeMahonyFilter_getOrientationEstimateAsRPY";
+  case 1665: return "AttitudeMahonyFilter_getInternalStateSize";
+  case 1666: return "AttitudeMahonyFilter_getInternalState";
+  case 1667: return "AttitudeMahonyFilter_getDefaultInternalInitialState";
+  case 1668: return "AttitudeMahonyFilter_setInternalState";
+  case 1669: return "AttitudeMahonyFilter_setInternalStateInitialOrientation";
+  case 1670: return "delete_AttitudeMahonyFilter";
+  case 1671: return "DiscreteExtendedKalmanFilterHelper_ekf_f";
+  case 1672: return "DiscreteExtendedKalmanFilterHelper_ekf_h";
+  case 1673: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF";
+  case 1674: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH";
+  case 1675: return "DiscreteExtendedKalmanFilterHelper_ekfPredict";
+  case 1676: return "DiscreteExtendedKalmanFilterHelper_ekfUpdate";
+  case 1677: return "DiscreteExtendedKalmanFilterHelper_ekfInit";
+  case 1678: return "DiscreteExtendedKalmanFilterHelper_ekfReset";
+  case 1679: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector";
+  case 1680: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector";
+  case 1681: return "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState";
+  case 1682: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance";
+  case 1683: return "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance";
+  case 1684: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance";
+  case 1685: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize";
+  case 1686: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize";
+  case 1687: return "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize";
+  case 1688: return "DiscreteExtendedKalmanFilterHelper_ekfGetStates";
+  case 1689: return "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance";
+  case 1690: return "delete_DiscreteExtendedKalmanFilterHelper";
+  case 1691: return "output_dimensions_with_magnetometer_get";
+  case 1692: return "output_dimensions_without_magnetometer_get";
+  case 1693: return "input_dimensions_get";
+  case 1694: return "AttitudeEstimatorState_m_orientation_get";
+  case 1695: return "AttitudeEstimatorState_m_orientation_set";
+  case 1696: return "AttitudeEstimatorState_m_angular_velocity_get";
+  case 1697: return "AttitudeEstimatorState_m_angular_velocity_set";
+  case 1698: return "AttitudeEstimatorState_m_gyroscope_bias_get";
+  case 1699: return "AttitudeEstimatorState_m_gyroscope_bias_set";
+  case 1700: return "new_AttitudeEstimatorState";
+  case 1701: return "delete_AttitudeEstimatorState";
+  case 1702: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_get";
+  case 1703: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_set";
+  case 1704: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get";
+  case 1705: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set";
+  case 1706: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get";
+  case 1707: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set";
+  case 1708: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get";
+  case 1709: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set";
+  case 1710: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get";
+  case 1711: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set";
+  case 1712: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get";
+  case 1713: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set";
+  case 1714: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get";
+  case 1715: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set";
+  case 1716: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get";
+  case 1717: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set";
+  case 1718: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get";
+  case 1719: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set";
+  case 1720: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get";
+  case 1721: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set";
+  case 1722: return "new_AttitudeQuaternionEKFParameters";
+  case 1723: return "delete_AttitudeQuaternionEKFParameters";
+  case 1724: return "new_AttitudeQuaternionEKF";
+  case 1725: return "AttitudeQuaternionEKF_getParameters";
+  case 1726: return "AttitudeQuaternionEKF_setParameters";
+  case 1727: return "AttitudeQuaternionEKF_setGravityDirection";
+  case 1728: return "AttitudeQuaternionEKF_setTimeStepInSeconds";
+  case 1729: return "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor";
+  case 1730: return "AttitudeQuaternionEKF_useMagnetometerMeasurements";
+  case 1731: return "AttitudeQuaternionEKF_setMeasurementNoiseVariance";
+  case 1732: return "AttitudeQuaternionEKF_setSystemNoiseVariance";
+  case 1733: return "AttitudeQuaternionEKF_setInitialStateCovariance";
+  case 1734: return "AttitudeQuaternionEKF_initializeFilter";
+  case 1735: return "AttitudeQuaternionEKF_updateFilterWithMeasurements";
+  case 1736: return "AttitudeQuaternionEKF_propagateStates";
+  case 1737: return "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix";
+  case 1738: return "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion";
+  case 1739: return "AttitudeQuaternionEKF_getOrientationEstimateAsRPY";
+  case 1740: return "AttitudeQuaternionEKF_getInternalStateSize";
+  case 1741: return "AttitudeQuaternionEKF_getInternalState";
+  case 1742: return "AttitudeQuaternionEKF_getDefaultInternalInitialState";
+  case 1743: return "AttitudeQuaternionEKF_setInternalState";
+  case 1744: return "AttitudeQuaternionEKF_setInternalStateInitialOrientation";
+  case 1745: return "delete_AttitudeQuaternionEKF";
+  case 1746: return "DynamicsRegressorParameter_category_get";
+  case 1747: return "DynamicsRegressorParameter_category_set";
+  case 1748: return "DynamicsRegressorParameter_elemIndex_get";
+  case 1749: return "DynamicsRegressorParameter_elemIndex_set";
+  case 1750: return "DynamicsRegressorParameter_type_get";
+  case 1751: return "DynamicsRegressorParameter_type_set";
+  case 1752: return "DynamicsRegressorParameter_lt";
+  case 1753: return "DynamicsRegressorParameter_eq";
+  case 1754: return "DynamicsRegressorParameter_ne";
+  case 1755: return "new_DynamicsRegressorParameter";
+  case 1756: return "delete_DynamicsRegressorParameter";
+  case 1757: return "DynamicsRegressorParametersList_parameters_get";
+  case 1758: return "DynamicsRegressorParametersList_parameters_set";
+  case 1759: return "DynamicsRegressorParametersList_getDescriptionOfParameter";
+  case 1760: return "DynamicsRegressorParametersList_addParam";
+  case 1761: return "DynamicsRegressorParametersList_addList";
+  case 1762: return "DynamicsRegressorParametersList_findParam";
+  case 1763: return "DynamicsRegressorParametersList_getNrOfParameters";
+  case 1764: return "new_DynamicsRegressorParametersList";
+  case 1765: return "delete_DynamicsRegressorParametersList";
+  case 1766: return "new_DynamicsRegressorGenerator";
+  case 1767: return "delete_DynamicsRegressorGenerator";
+  case 1768: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile";
+  case 1769: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString";
+  case 1770: return "DynamicsRegressorGenerator_loadRegressorStructureFromFile";
+  case 1771: return "DynamicsRegressorGenerator_loadRegressorStructureFromString";
+  case 1772: return "DynamicsRegressorGenerator_isValid";
+  case 1773: return "DynamicsRegressorGenerator_getNrOfParameters";
+  case 1774: return "DynamicsRegressorGenerator_getNrOfOutputs";
+  case 1775: return "DynamicsRegressorGenerator_getNrOfDegreesOfFreedom";
+  case 1776: return "DynamicsRegressorGenerator_getDescriptionOfParameter";
+  case 1777: return "DynamicsRegressorGenerator_getDescriptionOfParameters";
+  case 1778: return "DynamicsRegressorGenerator_getDescriptionOfOutput";
+  case 1779: return "DynamicsRegressorGenerator_getDescriptionOfOutputs";
+  case 1780: return "DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom";
+  case 1781: return "DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom";
+  case 1782: return "DynamicsRegressorGenerator_getDescriptionOfLink";
+  case 1783: return "DynamicsRegressorGenerator_getDescriptionOfLinks";
+  case 1784: return "DynamicsRegressorGenerator_getNrOfLinks";
+  case 1785: return "DynamicsRegressorGenerator_getNrOfFakeLinks";
+  case 1786: return "DynamicsRegressorGenerator_getBaseLinkName";
+  case 1787: return "DynamicsRegressorGenerator_getSensorsModel";
+  case 1788: return "DynamicsRegressorGenerator_setRobotState";
+  case 1789: return "DynamicsRegressorGenerator_getSensorsMeasurements";
+  case 1790: return "DynamicsRegressorGenerator_setTorqueSensorMeasurement";
+  case 1791: return "DynamicsRegressorGenerator_computeRegressor";
+  case 1792: return "DynamicsRegressorGenerator_getModelParameters";
+  case 1793: return "DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace";
+  case 1794: return "DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace";
+  case 1795: return "DynamicsRegressorGenerator_generate_random_regressors";
+  case 1796: return "new_KinDynComputations";
+  case 1797: return "delete_KinDynComputations";
+  case 1798: return "KinDynComputations_loadRobotModel";
+  case 1799: return "KinDynComputations_loadRobotModelFromFile";
+  case 1800: return "KinDynComputations_loadRobotModelFromString";
+  case 1801: return "KinDynComputations_isValid";
+  case 1802: return "KinDynComputations_setFrameVelocityRepresentation";
+  case 1803: return "KinDynComputations_getFrameVelocityRepresentation";
+  case 1804: return "KinDynComputations_getNrOfDegreesOfFreedom";
+  case 1805: return "KinDynComputations_getDescriptionOfDegreeOfFreedom";
+  case 1806: return "KinDynComputations_getDescriptionOfDegreesOfFreedom";
+  case 1807: return "KinDynComputations_getNrOfLinks";
+  case 1808: return "KinDynComputations_getNrOfFrames";
+  case 1809: return "KinDynComputations_getFloatingBase";
+  case 1810: return "KinDynComputations_setFloatingBase";
+  case 1811: return "KinDynComputations_model";
+  case 1812: return "KinDynComputations_getRobotModel";
+  case 1813: return "KinDynComputations_getRelativeJacobianSparsityPattern";
+  case 1814: return "KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern";
+  case 1815: return "KinDynComputations_setJointPos";
+  case 1816: return "KinDynComputations_setRobotState";
+  case 1817: return "KinDynComputations_getRobotState";
+  case 1818: return "KinDynComputations_getWorldBaseTransform";
+  case 1819: return "KinDynComputations_getBaseTwist";
+  case 1820: return "KinDynComputations_getJointPos";
+  case 1821: return "KinDynComputations_getJointVel";
+  case 1822: return "KinDynComputations_getModelVel";
+  case 1823: return "KinDynComputations_getFrameIndex";
+  case 1824: return "KinDynComputations_getFrameName";
+  case 1825: return "KinDynComputations_getWorldTransform";
+  case 1826: return "KinDynComputations_getRelativeTransformExplicit";
+  case 1827: return "KinDynComputations_getRelativeTransform";
+  case 1828: return "KinDynComputations_getFrameVel";
+  case 1829: return "KinDynComputations_getFrameAcc";
+  case 1830: return "KinDynComputations_getFrameFreeFloatingJacobian";
+  case 1831: return "KinDynComputations_getRelativeJacobian";
+  case 1832: return "KinDynComputations_getRelativeJacobianExplicit";
+  case 1833: return "KinDynComputations_getFrameBiasAcc";
+  case 1834: return "KinDynComputations_getCenterOfMassPosition";
+  case 1835: return "KinDynComputations_getCenterOfMassVelocity";
+  case 1836: return "KinDynComputations_getCenterOfMassJacobian";
+  case 1837: return "KinDynComputations_getCenterOfMassBiasAcc";
+  case 1838: return "KinDynComputations_getAverageVelocity";
+  case 1839: return "KinDynComputations_getAverageVelocityJacobian";
+  case 1840: return "KinDynComputations_getCentroidalAverageVelocity";
+  case 1841: return "KinDynComputations_getCentroidalAverageVelocityJacobian";
+  case 1842: return "KinDynComputations_getLinearAngularMomentum";
+  case 1843: return "KinDynComputations_getLinearAngularMomentumJacobian";
+  case 1844: return "KinDynComputations_getCentroidalTotalMomentum";
+  case 1845: return "KinDynComputations_getFreeFloatingMassMatrix";
+  case 1846: return "KinDynComputations_inverseDynamics";
+  case 1847: return "KinDynComputations_generalizedBiasForces";
+  case 1848: return "KinDynComputations_generalizedGravityForces";
+  case 1849: return "KinDynComputations_generalizedExternalForces";
+  case 1850: return "KinDynComputations_inverseDynamicsInertialParametersRegressor";
+  case 1851: return "delete_ICamera";
+  case 1852: return "ICamera_setPosition";
+  case 1853: return "ICamera_setTarget";
+  case 1854: return "ICamera_setUpVector";
+  case 1855: return "ColorViz_r_get";
+  case 1856: return "ColorViz_r_set";
+  case 1857: return "ColorViz_g_get";
+  case 1858: return "ColorViz_g_set";
+  case 1859: return "ColorViz_b_get";
+  case 1860: return "ColorViz_b_set";
+  case 1861: return "ColorViz_a_get";
+  case 1862: return "ColorViz_a_set";
+  case 1863: return "new_ColorViz";
+  case 1864: return "delete_ColorViz";
+  case 1865: return "delete_ILight";
+  case 1866: return "ILight_getName";
+  case 1867: return "ILight_setType";
+  case 1868: return "ILight_getType";
+  case 1869: return "ILight_setPosition";
+  case 1870: return "ILight_getPosition";
+  case 1871: return "ILight_setDirection";
+  case 1872: return "ILight_getDirection";
+  case 1873: return "ILight_setAmbientColor";
+  case 1874: return "ILight_getAmbientColor";
+  case 1875: return "ILight_setSpecularColor";
+  case 1876: return "ILight_getSpecularColor";
+  case 1877: return "ILight_setDiffuseColor";
+  case 1878: return "ILight_getDiffuseColor";
+  case 1879: return "delete_IEnvironment";
+  case 1880: return "IEnvironment_getElements";
+  case 1881: return "IEnvironment_setElementVisibility";
+  case 1882: return "IEnvironment_setBackgroundColor";
+  case 1883: return "IEnvironment_setAmbientLight";
+  case 1884: return "IEnvironment_getLights";
+  case 1885: return "IEnvironment_addLight";
+  case 1886: return "IEnvironment_lightViz";
+  case 1887: return "IEnvironment_removeLight";
+  case 1888: return "delete_IJetsVisualization";
+  case 1889: return "IJetsVisualization_setJetsFrames";
+  case 1890: return "IJetsVisualization_getNrOfJets";
+  case 1891: return "IJetsVisualization_getJetDirection";
+  case 1892: return "IJetsVisualization_setJetDirection";
+  case 1893: return "IJetsVisualization_setJetColor";
+  case 1894: return "IJetsVisualization_setJetsDimensions";
+  case 1895: return "IJetsVisualization_setJetsIntensity";
+  case 1896: return "delete_IVectorsVisualization";
+  case 1897: return "IVectorsVisualization_addVector";
+  case 1898: return "IVectorsVisualization_getNrOfVectors";
+  case 1899: return "IVectorsVisualization_getVector";
+  case 1900: return "IVectorsVisualization_updateVector";
+  case 1901: return "IVectorsVisualization_setVectorColor";
+  case 1902: return "IVectorsVisualization_setVectorsAspect";
+  case 1903: return "delete_IModelVisualization";
+  case 1904: return "IModelVisualization_setPositions";
+  case 1905: return "IModelVisualization_setLinkPositions";
+  case 1906: return "IModelVisualization_model";
+  case 1907: return "IModelVisualization_getInstanceName";
+  case 1908: return "IModelVisualization_setModelVisibility";
+  case 1909: return "IModelVisualization_setModelColor";
+  case 1910: return "IModelVisualization_resetModelColor";
+  case 1911: return "IModelVisualization_setLinkColor";
+  case 1912: return "IModelVisualization_resetLinkColor";
+  case 1913: return "IModelVisualization_getLinkNames";
+  case 1914: return "IModelVisualization_setLinkVisibility";
+  case 1915: return "IModelVisualization_getFeatures";
+  case 1916: return "IModelVisualization_setFeatureVisibility";
+  case 1917: return "IModelVisualization_jets";
+  case 1918: return "IModelVisualization_getWorldModelTransform";
+  case 1919: return "IModelVisualization_getWorldLinkTransform";
+  case 1920: return "VisualizerOptions_verbose_get";
+  case 1921: return "VisualizerOptions_verbose_set";
+  case 1922: return "VisualizerOptions_winWidth_get";
+  case 1923: return "VisualizerOptions_winWidth_set";
+  case 1924: return "VisualizerOptions_winHeight_get";
+  case 1925: return "VisualizerOptions_winHeight_set";
+  case 1926: return "VisualizerOptions_rootFrameArrowsDimension_get";
+  case 1927: return "VisualizerOptions_rootFrameArrowsDimension_set";
+  case 1928: return "new_VisualizerOptions";
+  case 1929: return "delete_VisualizerOptions";
+  case 1930: return "new_Visualizer";
+  case 1931: return "delete_Visualizer";
+  case 1932: return "Visualizer_init";
+  case 1933: return "Visualizer_getNrOfVisualizedModels";
+  case 1934: return "Visualizer_getModelInstanceName";
+  case 1935: return "Visualizer_getModelInstanceIndex";
+  case 1936: return "Visualizer_addModel";
+  case 1937: return "Visualizer_modelViz";
+  case 1938: return "Visualizer_camera";
+  case 1939: return "Visualizer_enviroment";
+  case 1940: return "Visualizer_vectors";
+  case 1941: return "Visualizer_run";
+  case 1942: return "Visualizer_draw";
+  case 1943: return "Visualizer_drawToFile";
+  case 1944: return "Visualizer_close";
+  case 1945: return "new_DynamicsComputations";
+  case 1946: return "delete_DynamicsComputations";
+  case 1947: return "DynamicsComputations_loadRobotModelFromFile";
+  case 1948: return "DynamicsComputations_loadRobotModelFromString";
+  case 1949: return "DynamicsComputations_isValid";
+  case 1950: return "DynamicsComputations_getNrOfDegreesOfFreedom";
+  case 1951: return "DynamicsComputations_getDescriptionOfDegreeOfFreedom";
+  case 1952: return "DynamicsComputations_getDescriptionOfDegreesOfFreedom";
+  case 1953: return "DynamicsComputations_getNrOfLinks";
+  case 1954: return "DynamicsComputations_getNrOfFrames";
+  case 1955: return "DynamicsComputations_getFloatingBase";
+  case 1956: return "DynamicsComputations_setFloatingBase";
+  case 1957: return "DynamicsComputations_setRobotState";
+  case 1958: return "DynamicsComputations_getWorldBaseTransform";
+  case 1959: return "DynamicsComputations_getBaseTwist";
+  case 1960: return "DynamicsComputations_getJointPos";
+  case 1961: return "DynamicsComputations_getJointVel";
+  case 1962: return "DynamicsComputations_getFrameIndex";
+  case 1963: return "DynamicsComputations_getFrameName";
+  case 1964: return "DynamicsComputations_getWorldTransform";
+  case 1965: return "DynamicsComputations_getRelativeTransform";
+  case 1966: return "DynamicsComputations_getFrameTwist";
+  case 1967: return "DynamicsComputations_getFrameTwistInWorldOrient";
+  case 1968: return "DynamicsComputations_getFrameProperSpatialAcceleration";
+  case 1969: return "DynamicsComputations_getLinkIndex";
+  case 1970: return "DynamicsComputations_getLinkInertia";
+  case 1971: return "DynamicsComputations_getJointIndex";
+  case 1972: return "DynamicsComputations_getJointName";
+  case 1973: return "DynamicsComputations_getJointLimits";
+  case 1974: return "DynamicsComputations_inverseDynamics";
+  case 1975: return "DynamicsComputations_getFreeFloatingMassMatrix";
+  case 1976: return "DynamicsComputations_getFrameJacobian";
+  case 1977: return "DynamicsComputations_getDynamicsRegressor";
+  case 1978: return "DynamicsComputations_getModelDynamicsParameters";
+  case 1979: return "DynamicsComputations_getCenterOfMass";
+  case 1980: return "DynamicsComputations_getCenterOfMassJacobian";
   default: return 0;
   }
 }
@@ -91814,1014 +98134,1171 @@ void mexFunction(int resc, mxArray *resv[], int argc, const mxArray *argv[]) {
   case 813: flag=_wrap_TransformDerivative_mtimes(resc,resv,argc,(mxArray**)(argv)); break;
   case 814: flag=_wrap_TransformDerivative_derivativeOfInverse(resc,resv,argc,(mxArray**)(argv)); break;
   case 815: flag=_wrap_TransformDerivative_transform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 816: flag=_wrap_LINK_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 817: flag=_wrap_LINK_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 818: flag=_wrap_LINK_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 819: flag=_wrap_LINK_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 820: flag=_wrap_JOINT_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 821: flag=_wrap_JOINT_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 822: flag=_wrap_JOINT_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 823: flag=_wrap_JOINT_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 824: flag=_wrap_DOF_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 825: flag=_wrap_DOF_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 826: flag=_wrap_DOF_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 827: flag=_wrap_DOF_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 828: flag=_wrap_FRAME_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 829: flag=_wrap_FRAME_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 830: flag=_wrap_FRAME_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 831: flag=_wrap_FRAME_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 832: flag=_wrap_TRAVERSAL_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 833: flag=_wrap_TRAVERSAL_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 834: flag=_wrap_new_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 835: flag=_wrap_LinkPositions_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 836: flag=_wrap_LinkPositions_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 837: flag=_wrap_LinkPositions_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 838: flag=_wrap_LinkPositions_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 839: flag=_wrap_LinkPositions_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 840: flag=_wrap_delete_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 841: flag=_wrap_new_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 842: flag=_wrap_LinkWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 843: flag=_wrap_LinkWrenches_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 844: flag=_wrap_LinkWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 845: flag=_wrap_LinkWrenches_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 846: flag=_wrap_LinkWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 847: flag=_wrap_LinkWrenches_zero(resc,resv,argc,(mxArray**)(argv)); break;
-  case 848: flag=_wrap_delete_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 849: flag=_wrap_new_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 850: flag=_wrap_LinkInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 851: flag=_wrap_LinkInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 852: flag=_wrap_LinkInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 853: flag=_wrap_delete_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 854: flag=_wrap_new_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 855: flag=_wrap_LinkArticulatedBodyInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 856: flag=_wrap_LinkArticulatedBodyInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 857: flag=_wrap_LinkArticulatedBodyInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 858: flag=_wrap_delete_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 859: flag=_wrap_new_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 860: flag=_wrap_LinkVelArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 861: flag=_wrap_LinkVelArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 862: flag=_wrap_LinkVelArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 863: flag=_wrap_LinkVelArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 864: flag=_wrap_LinkVelArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 865: flag=_wrap_delete_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 866: flag=_wrap_new_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 867: flag=_wrap_LinkAccArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 868: flag=_wrap_LinkAccArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 869: flag=_wrap_LinkAccArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 870: flag=_wrap_LinkAccArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 871: flag=_wrap_LinkAccArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 872: flag=_wrap_delete_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 873: flag=_wrap_new_Link(resc,resv,argc,(mxArray**)(argv)); break;
-  case 874: flag=_wrap_Link_inertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 875: flag=_wrap_Link_setInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 876: flag=_wrap_Link_getInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 877: flag=_wrap_Link_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 878: flag=_wrap_Link_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 879: flag=_wrap_delete_Link(resc,resv,argc,(mxArray**)(argv)); break;
-  case 880: flag=_wrap_delete_IJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 881: flag=_wrap_IJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 882: flag=_wrap_IJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 883: flag=_wrap_IJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 884: flag=_wrap_IJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 885: flag=_wrap_IJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 886: flag=_wrap_IJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 887: flag=_wrap_IJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 888: flag=_wrap_IJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 889: flag=_wrap_IJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 890: flag=_wrap_IJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 891: flag=_wrap_IJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 892: flag=_wrap_IJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 893: flag=_wrap_IJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 894: flag=_wrap_IJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 895: flag=_wrap_IJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 896: flag=_wrap_IJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 897: flag=_wrap_IJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 898: flag=_wrap_IJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 899: flag=_wrap_IJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 900: flag=_wrap_IJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 901: flag=_wrap_IJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 902: flag=_wrap_IJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 903: flag=_wrap_IJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 904: flag=_wrap_IJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 905: flag=_wrap_IJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 906: flag=_wrap_IJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 907: flag=_wrap_IJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 908: flag=_wrap_IJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 909: flag=_wrap_IJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 910: flag=_wrap_IJoint_isRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 911: flag=_wrap_IJoint_isFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 912: flag=_wrap_IJoint_asRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 913: flag=_wrap_IJoint_asFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 914: flag=_wrap_new_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 915: flag=_wrap_delete_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 916: flag=_wrap_FixedJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 917: flag=_wrap_FixedJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 918: flag=_wrap_FixedJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 919: flag=_wrap_FixedJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 920: flag=_wrap_FixedJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 921: flag=_wrap_FixedJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 922: flag=_wrap_FixedJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 923: flag=_wrap_FixedJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 924: flag=_wrap_FixedJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 925: flag=_wrap_FixedJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 926: flag=_wrap_FixedJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 927: flag=_wrap_FixedJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 928: flag=_wrap_FixedJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 929: flag=_wrap_FixedJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 930: flag=_wrap_FixedJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 931: flag=_wrap_FixedJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 932: flag=_wrap_FixedJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 933: flag=_wrap_FixedJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 934: flag=_wrap_FixedJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 935: flag=_wrap_FixedJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 936: flag=_wrap_FixedJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 937: flag=_wrap_FixedJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 938: flag=_wrap_FixedJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 939: flag=_wrap_FixedJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 940: flag=_wrap_FixedJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 941: flag=_wrap_FixedJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 942: flag=_wrap_FixedJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 943: flag=_wrap_FixedJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 944: flag=_wrap_FixedJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 945: flag=_wrap_delete_MovableJointImpl1(resc,resv,argc,(mxArray**)(argv)); break;
-  case 946: flag=_wrap_MovableJointImpl1_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 947: flag=_wrap_MovableJointImpl1_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 948: flag=_wrap_MovableJointImpl1_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 949: flag=_wrap_MovableJointImpl1_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 950: flag=_wrap_MovableJointImpl1_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 951: flag=_wrap_MovableJointImpl1_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 952: flag=_wrap_MovableJointImpl1_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 953: flag=_wrap_MovableJointImpl1_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 954: flag=_wrap_delete_MovableJointImpl2(resc,resv,argc,(mxArray**)(argv)); break;
-  case 955: flag=_wrap_MovableJointImpl2_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 956: flag=_wrap_MovableJointImpl2_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 957: flag=_wrap_MovableJointImpl2_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 958: flag=_wrap_MovableJointImpl2_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 959: flag=_wrap_MovableJointImpl2_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 960: flag=_wrap_MovableJointImpl2_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 961: flag=_wrap_MovableJointImpl2_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 962: flag=_wrap_MovableJointImpl2_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 963: flag=_wrap_delete_MovableJointImpl3(resc,resv,argc,(mxArray**)(argv)); break;
-  case 964: flag=_wrap_MovableJointImpl3_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 965: flag=_wrap_MovableJointImpl3_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 966: flag=_wrap_MovableJointImpl3_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 967: flag=_wrap_MovableJointImpl3_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 968: flag=_wrap_MovableJointImpl3_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 969: flag=_wrap_MovableJointImpl3_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 970: flag=_wrap_MovableJointImpl3_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 971: flag=_wrap_MovableJointImpl3_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 972: flag=_wrap_delete_MovableJointImpl4(resc,resv,argc,(mxArray**)(argv)); break;
-  case 973: flag=_wrap_MovableJointImpl4_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 974: flag=_wrap_MovableJointImpl4_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 975: flag=_wrap_MovableJointImpl4_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 976: flag=_wrap_MovableJointImpl4_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 977: flag=_wrap_MovableJointImpl4_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 978: flag=_wrap_MovableJointImpl4_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 979: flag=_wrap_MovableJointImpl4_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 980: flag=_wrap_MovableJointImpl4_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 981: flag=_wrap_delete_MovableJointImpl5(resc,resv,argc,(mxArray**)(argv)); break;
-  case 982: flag=_wrap_MovableJointImpl5_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 983: flag=_wrap_MovableJointImpl5_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 984: flag=_wrap_MovableJointImpl5_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 985: flag=_wrap_MovableJointImpl5_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 986: flag=_wrap_MovableJointImpl5_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 987: flag=_wrap_MovableJointImpl5_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 988: flag=_wrap_MovableJointImpl5_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 989: flag=_wrap_MovableJointImpl5_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 990: flag=_wrap_delete_MovableJointImpl6(resc,resv,argc,(mxArray**)(argv)); break;
-  case 991: flag=_wrap_MovableJointImpl6_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 992: flag=_wrap_MovableJointImpl6_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 993: flag=_wrap_MovableJointImpl6_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 994: flag=_wrap_MovableJointImpl6_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 995: flag=_wrap_MovableJointImpl6_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 996: flag=_wrap_MovableJointImpl6_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 997: flag=_wrap_MovableJointImpl6_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 998: flag=_wrap_MovableJointImpl6_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 999: flag=_wrap_new_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1000: flag=_wrap_delete_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1001: flag=_wrap_RevoluteJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1002: flag=_wrap_RevoluteJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1003: flag=_wrap_RevoluteJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1004: flag=_wrap_RevoluteJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1005: flag=_wrap_RevoluteJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1006: flag=_wrap_RevoluteJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1007: flag=_wrap_RevoluteJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1008: flag=_wrap_RevoluteJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1009: flag=_wrap_RevoluteJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1010: flag=_wrap_RevoluteJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1011: flag=_wrap_RevoluteJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1012: flag=_wrap_RevoluteJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1013: flag=_wrap_RevoluteJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1014: flag=_wrap_RevoluteJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1015: flag=_wrap_RevoluteJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1016: flag=_wrap_RevoluteJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1017: flag=_wrap_RevoluteJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1018: flag=_wrap_RevoluteJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1019: flag=_wrap_RevoluteJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1020: flag=_wrap_RevoluteJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1021: flag=_wrap_RevoluteJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1022: flag=_wrap_RevoluteJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1023: flag=_wrap_RevoluteJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1024: flag=_wrap_new_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1025: flag=_wrap_delete_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1026: flag=_wrap_PrismaticJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1027: flag=_wrap_PrismaticJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1028: flag=_wrap_PrismaticJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1029: flag=_wrap_PrismaticJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1030: flag=_wrap_PrismaticJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1031: flag=_wrap_PrismaticJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1032: flag=_wrap_PrismaticJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1033: flag=_wrap_PrismaticJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1034: flag=_wrap_PrismaticJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1035: flag=_wrap_PrismaticJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1036: flag=_wrap_PrismaticJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1037: flag=_wrap_PrismaticJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1038: flag=_wrap_PrismaticJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1039: flag=_wrap_PrismaticJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1040: flag=_wrap_PrismaticJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1041: flag=_wrap_PrismaticJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1042: flag=_wrap_PrismaticJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1043: flag=_wrap_PrismaticJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1044: flag=_wrap_PrismaticJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1045: flag=_wrap_PrismaticJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1046: flag=_wrap_PrismaticJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1047: flag=_wrap_PrismaticJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1048: flag=_wrap_PrismaticJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1049: flag=_wrap_new_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1050: flag=_wrap_delete_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1051: flag=_wrap_Traversal_getNrOfVisitedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1052: flag=_wrap_Traversal_getLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1053: flag=_wrap_Traversal_getBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1054: flag=_wrap_Traversal_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1055: flag=_wrap_Traversal_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1056: flag=_wrap_Traversal_getParentLinkFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1057: flag=_wrap_Traversal_getParentJointFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1058: flag=_wrap_Traversal_getTraversalIndexFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1059: flag=_wrap_Traversal_reset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1060: flag=_wrap_Traversal_addTraversalBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1061: flag=_wrap_Traversal_addTraversalElement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1062: flag=_wrap_Traversal_isParentOf(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1063: flag=_wrap_Traversal_getChildLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1064: flag=_wrap_Traversal_getParentLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1065: flag=_wrap_Traversal_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1066: flag=_wrap_delete_SolidShape(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1067: flag=_wrap_SolidShape_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1068: flag=_wrap_SolidShape_name_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1069: flag=_wrap_SolidShape_name_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1070: flag=_wrap_SolidShape_link_H_geometry_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1071: flag=_wrap_SolidShape_link_H_geometry_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1072: flag=_wrap_SolidShape_material_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1073: flag=_wrap_SolidShape_material_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1074: flag=_wrap_SolidShape_isSphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1075: flag=_wrap_SolidShape_isBox(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1076: flag=_wrap_SolidShape_isCylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1077: flag=_wrap_SolidShape_isExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1078: flag=_wrap_SolidShape_asSphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1079: flag=_wrap_SolidShape_asBox(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1080: flag=_wrap_SolidShape_asCylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1081: flag=_wrap_SolidShape_asExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1082: flag=_wrap_delete_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1083: flag=_wrap_Sphere_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1084: flag=_wrap_Sphere_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1085: flag=_wrap_Sphere_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1086: flag=_wrap_new_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1087: flag=_wrap_delete_Box(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1088: flag=_wrap_Box_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1089: flag=_wrap_Box_x_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1090: flag=_wrap_Box_x_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1091: flag=_wrap_Box_y_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1092: flag=_wrap_Box_y_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1093: flag=_wrap_Box_z_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1094: flag=_wrap_Box_z_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1095: flag=_wrap_new_Box(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1096: flag=_wrap_delete_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1097: flag=_wrap_Cylinder_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1098: flag=_wrap_Cylinder_length_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1099: flag=_wrap_Cylinder_length_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1100: flag=_wrap_Cylinder_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1101: flag=_wrap_Cylinder_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1102: flag=_wrap_new_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1103: flag=_wrap_delete_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1104: flag=_wrap_ExternalMesh_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1105: flag=_wrap_ExternalMesh_filename_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1106: flag=_wrap_ExternalMesh_filename_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1107: flag=_wrap_ExternalMesh_scale_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1108: flag=_wrap_ExternalMesh_scale_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1109: flag=_wrap_new_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1110: flag=_wrap_new_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1111: flag=_wrap_ModelSolidShapes_clear(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1112: flag=_wrap_delete_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1113: flag=_wrap_ModelSolidShapes_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1114: flag=_wrap_ModelSolidShapes_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1115: flag=_wrap_ModelSolidShapes_linkSolidShapes_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1116: flag=_wrap_ModelSolidShapes_linkSolidShapes_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1117: flag=_wrap_Neighbor_neighborLink_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1118: flag=_wrap_Neighbor_neighborLink_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1119: flag=_wrap_Neighbor_neighborJoint_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1120: flag=_wrap_Neighbor_neighborJoint_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1121: flag=_wrap_new_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1122: flag=_wrap_delete_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1123: flag=_wrap_new_Model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1124: flag=_wrap_Model_copy(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1125: flag=_wrap_delete_Model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1126: flag=_wrap_Model_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1127: flag=_wrap_Model_getLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1128: flag=_wrap_Model_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1129: flag=_wrap_Model_isValidLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1130: flag=_wrap_Model_getLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1131: flag=_wrap_Model_addLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1132: flag=_wrap_Model_getNrOfJoints(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1133: flag=_wrap_Model_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1134: flag=_wrap_Model_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1135: flag=_wrap_Model_getJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1136: flag=_wrap_Model_isValidJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1137: flag=_wrap_Model_isLinkNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1138: flag=_wrap_Model_isJointNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1139: flag=_wrap_Model_isFrameNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1140: flag=_wrap_Model_addJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1141: flag=_wrap_Model_addJointAndLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1142: flag=_wrap_Model_insertLinkToExistingJointAndAddJointForDisplacedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1143: flag=_wrap_Model_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1144: flag=_wrap_Model_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1145: flag=_wrap_Model_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1146: flag=_wrap_Model_addAdditionalFrameToLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1147: flag=_wrap_Model_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1148: flag=_wrap_Model_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1149: flag=_wrap_Model_isValidFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1150: flag=_wrap_Model_getFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1151: flag=_wrap_Model_getFrameLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1152: flag=_wrap_Model_getNrOfNeighbors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1153: flag=_wrap_Model_getNeighbor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1154: flag=_wrap_Model_setDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1155: flag=_wrap_Model_getDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1156: flag=_wrap_Model_computeFullTreeTraversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1157: flag=_wrap_Model_getInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1158: flag=_wrap_Model_updateInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1159: flag=_wrap_Model_visualSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1160: flag=_wrap_Model_collisionSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1161: flag=_wrap_Model_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1162: flag=_wrap_new_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1163: flag=_wrap_JointPosDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1164: flag=_wrap_JointPosDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1165: flag=_wrap_delete_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1166: flag=_wrap_new_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1167: flag=_wrap_JointDOFsDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1168: flag=_wrap_JointDOFsDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1169: flag=_wrap_delete_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1170: flag=_wrap_new_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1171: flag=_wrap_DOFSpatialForceArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1172: flag=_wrap_DOFSpatialForceArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1173: flag=_wrap_DOFSpatialForceArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1174: flag=_wrap_delete_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1175: flag=_wrap_new_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1176: flag=_wrap_DOFSpatialMotionArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1177: flag=_wrap_DOFSpatialMotionArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1178: flag=_wrap_DOFSpatialMotionArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1179: flag=_wrap_delete_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1180: flag=_wrap_new_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1181: flag=_wrap_FrameFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1182: flag=_wrap_FrameFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1183: flag=_wrap_delete_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1184: flag=_wrap_new_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1185: flag=_wrap_MomentumFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1186: flag=_wrap_MomentumFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1187: flag=_wrap_delete_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1188: flag=_wrap_new_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1189: flag=_wrap_FreeFloatingMassMatrix_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1190: flag=_wrap_delete_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1191: flag=_wrap_new_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1192: flag=_wrap_FreeFloatingPos_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1193: flag=_wrap_FreeFloatingPos_worldBasePos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1194: flag=_wrap_FreeFloatingPos_jointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1195: flag=_wrap_FreeFloatingPos_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1196: flag=_wrap_delete_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1197: flag=_wrap_new_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1198: flag=_wrap_FreeFloatingGeneralizedTorques_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1199: flag=_wrap_FreeFloatingGeneralizedTorques_baseWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1200: flag=_wrap_FreeFloatingGeneralizedTorques_jointTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1201: flag=_wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1202: flag=_wrap_delete_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1203: flag=_wrap_new_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1204: flag=_wrap_FreeFloatingVel_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1205: flag=_wrap_FreeFloatingVel_baseVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1206: flag=_wrap_FreeFloatingVel_jointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1207: flag=_wrap_FreeFloatingVel_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1208: flag=_wrap_delete_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1209: flag=_wrap_new_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1210: flag=_wrap_FreeFloatingAcc_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1211: flag=_wrap_FreeFloatingAcc_baseAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1212: flag=_wrap_FreeFloatingAcc_jointAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1213: flag=_wrap_FreeFloatingAcc_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1214: flag=_wrap_delete_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1215: flag=_wrap_ContactWrench_contactId(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1216: flag=_wrap_ContactWrench_contactPoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1217: flag=_wrap_ContactWrench_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1218: flag=_wrap_new_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1219: flag=_wrap_delete_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1220: flag=_wrap_new_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1221: flag=_wrap_LinkContactWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1222: flag=_wrap_LinkContactWrenches_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1223: flag=_wrap_LinkContactWrenches_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1224: flag=_wrap_LinkContactWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1225: flag=_wrap_LinkContactWrenches_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1226: flag=_wrap_LinkContactWrenches_computeNetWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1227: flag=_wrap_LinkContactWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1228: flag=_wrap_delete_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1229: flag=_wrap_ForwardPositionKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1230: flag=_wrap_ForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1231: flag=_wrap_ForwardPosVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1232: flag=_wrap_ForwardPosVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1233: flag=_wrap_ForwardAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1234: flag=_wrap_ForwardBiasAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1235: flag=_wrap_ComputeLinearAndAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1236: flag=_wrap_ComputeLinearAndAngularMomentumDerivativeBias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1237: flag=_wrap_RNEADynamicPhase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1238: flag=_wrap_CompositeRigidBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1239: flag=_wrap_new_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1240: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1241: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1242: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1243: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1244: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1245: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1246: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1247: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1248: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1249: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1250: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1251: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1252: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1253: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1254: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1255: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1256: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1257: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1258: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1259: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1260: flag=_wrap_delete_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1261: flag=_wrap_ArticulatedBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1262: flag=_wrap_NR_OF_SENSOR_TYPES_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1263: flag=_wrap_isLinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1264: flag=_wrap_isJointSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1265: flag=_wrap_getSensorTypeSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1266: flag=_wrap_delete_Sensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1267: flag=_wrap_Sensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1268: flag=_wrap_Sensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1269: flag=_wrap_Sensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1270: flag=_wrap_Sensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1271: flag=_wrap_Sensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1272: flag=_wrap_Sensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1273: flag=_wrap_Sensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1274: flag=_wrap_Sensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1275: flag=_wrap_delete_JointSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1276: flag=_wrap_JointSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1277: flag=_wrap_JointSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1278: flag=_wrap_JointSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1279: flag=_wrap_JointSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1280: flag=_wrap_JointSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1281: flag=_wrap_delete_LinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1282: flag=_wrap_LinkSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1283: flag=_wrap_LinkSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1284: flag=_wrap_LinkSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1285: flag=_wrap_LinkSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1286: flag=_wrap_LinkSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1287: flag=_wrap_LinkSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1288: flag=_wrap_LinkSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1289: flag=_wrap_new_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1290: flag=_wrap_delete_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1291: flag=_wrap_SensorsList_addSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1292: flag=_wrap_SensorsList_setSerialization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1293: flag=_wrap_SensorsList_getSerialization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1294: flag=_wrap_SensorsList_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1295: flag=_wrap_SensorsList_getSensorIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1296: flag=_wrap_SensorsList_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1297: flag=_wrap_SensorsList_getSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1298: flag=_wrap_SensorsList_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1299: flag=_wrap_SensorsList_removeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1300: flag=_wrap_SensorsList_removeAllSensorsOfType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1301: flag=_wrap_SensorsList_getSixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1302: flag=_wrap_SensorsList_getAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1303: flag=_wrap_SensorsList_getGyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1304: flag=_wrap_SensorsList_getThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1305: flag=_wrap_SensorsList_getThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1306: flag=_wrap_new_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1307: flag=_wrap_delete_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1308: flag=_wrap_SensorsMeasurements_setNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1309: flag=_wrap_SensorsMeasurements_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1310: flag=_wrap_SensorsMeasurements_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1311: flag=_wrap_SensorsMeasurements_toVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1312: flag=_wrap_SensorsMeasurements_setMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1313: flag=_wrap_SensorsMeasurements_getMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1314: flag=_wrap_SensorsMeasurements_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1315: flag=_wrap_new_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1316: flag=_wrap_delete_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1317: flag=_wrap_SixAxisForceTorqueSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1318: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1319: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1320: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1321: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1322: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1323: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1324: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1325: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1326: flag=_wrap_SixAxisForceTorqueSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1327: flag=_wrap_SixAxisForceTorqueSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1328: flag=_wrap_SixAxisForceTorqueSensor_setAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1329: flag=_wrap_SixAxisForceTorqueSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1330: flag=_wrap_SixAxisForceTorqueSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1331: flag=_wrap_SixAxisForceTorqueSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1332: flag=_wrap_SixAxisForceTorqueSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1333: flag=_wrap_SixAxisForceTorqueSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1334: flag=_wrap_SixAxisForceTorqueSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1335: flag=_wrap_SixAxisForceTorqueSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1336: flag=_wrap_SixAxisForceTorqueSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1337: flag=_wrap_SixAxisForceTorqueSensor_getAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1338: flag=_wrap_SixAxisForceTorqueSensor_isLinkAttachedToSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1339: flag=_wrap_SixAxisForceTorqueSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1340: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1341: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1342: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1343: flag=_wrap_SixAxisForceTorqueSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1344: flag=_wrap_SixAxisForceTorqueSensor_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1345: flag=_wrap_new_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1346: flag=_wrap_delete_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1347: flag=_wrap_AccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1348: flag=_wrap_AccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1349: flag=_wrap_AccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1350: flag=_wrap_AccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1351: flag=_wrap_AccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1352: flag=_wrap_AccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1353: flag=_wrap_AccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1354: flag=_wrap_AccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1355: flag=_wrap_AccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1356: flag=_wrap_AccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1357: flag=_wrap_AccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1358: flag=_wrap_AccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1359: flag=_wrap_AccelerometerSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1360: flag=_wrap_AccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1361: flag=_wrap_new_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1362: flag=_wrap_delete_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1363: flag=_wrap_GyroscopeSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1364: flag=_wrap_GyroscopeSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1365: flag=_wrap_GyroscopeSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1366: flag=_wrap_GyroscopeSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1367: flag=_wrap_GyroscopeSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1368: flag=_wrap_GyroscopeSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1369: flag=_wrap_GyroscopeSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1370: flag=_wrap_GyroscopeSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1371: flag=_wrap_GyroscopeSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1372: flag=_wrap_GyroscopeSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1373: flag=_wrap_GyroscopeSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1374: flag=_wrap_GyroscopeSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1375: flag=_wrap_GyroscopeSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1376: flag=_wrap_GyroscopeSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1377: flag=_wrap_new_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1378: flag=_wrap_delete_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1379: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1380: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1381: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1382: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1383: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1384: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1385: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1386: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1387: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1388: flag=_wrap_ThreeAxisAngularAccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1389: flag=_wrap_ThreeAxisAngularAccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1390: flag=_wrap_ThreeAxisAngularAccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1391: flag=_wrap_ThreeAxisAngularAccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1392: flag=_wrap_new_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1393: flag=_wrap_delete_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1394: flag=_wrap_ThreeAxisForceTorqueContactSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1395: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1396: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1397: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1398: flag=_wrap_ThreeAxisForceTorqueContactSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1399: flag=_wrap_ThreeAxisForceTorqueContactSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1400: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1401: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1402: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1403: flag=_wrap_ThreeAxisForceTorqueContactSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1404: flag=_wrap_ThreeAxisForceTorqueContactSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1405: flag=_wrap_ThreeAxisForceTorqueContactSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1406: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1407: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1408: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1409: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1410: flag=_wrap_predictSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1411: flag=_wrap_predictSensorsMeasurementsFromRawBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1412: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1413: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1414: flag=_wrap_URDFParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1415: flag=_wrap_URDFParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1416: flag=_wrap_new_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1417: flag=_wrap_delete_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1418: flag=_wrap_modelFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1419: flag=_wrap_modelFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1420: flag=_wrap_dofsListFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1421: flag=_wrap_dofsListFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1422: flag=_wrap_sensorsFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1423: flag=_wrap_sensorsFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1424: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1425: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1426: flag=_wrap_ModelParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1427: flag=_wrap_ModelParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1428: flag=_wrap_new_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1429: flag=_wrap_delete_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1430: flag=_wrap_new_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1431: flag=_wrap_delete_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1432: flag=_wrap_ModelLoader_parsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1433: flag=_wrap_ModelLoader_setParsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1434: flag=_wrap_ModelLoader_loadModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1435: flag=_wrap_ModelLoader_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1436: flag=_wrap_ModelLoader_loadReducedModelFromFullModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1437: flag=_wrap_ModelLoader_loadReducedModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1438: flag=_wrap_ModelLoader_loadReducedModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1439: flag=_wrap_ModelLoader_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1440: flag=_wrap_ModelLoader_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1441: flag=_wrap_ModelLoader_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1442: flag=_wrap_new_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1443: flag=_wrap_UnknownWrenchContact_unknownType_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1444: flag=_wrap_UnknownWrenchContact_unknownType_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1445: flag=_wrap_UnknownWrenchContact_contactPoint_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1446: flag=_wrap_UnknownWrenchContact_contactPoint_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1447: flag=_wrap_UnknownWrenchContact_forceDirection_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1448: flag=_wrap_UnknownWrenchContact_forceDirection_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1449: flag=_wrap_UnknownWrenchContact_knownWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1450: flag=_wrap_UnknownWrenchContact_knownWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1451: flag=_wrap_UnknownWrenchContact_contactId_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1452: flag=_wrap_UnknownWrenchContact_contactId_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1453: flag=_wrap_delete_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1454: flag=_wrap_new_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1455: flag=_wrap_LinkUnknownWrenchContacts_clear(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1456: flag=_wrap_LinkUnknownWrenchContacts_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1457: flag=_wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1458: flag=_wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1459: flag=_wrap_LinkUnknownWrenchContacts_addNewContactForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1460: flag=_wrap_LinkUnknownWrenchContacts_addNewContactInFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1461: flag=_wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1462: flag=_wrap_LinkUnknownWrenchContacts_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1463: flag=_wrap_LinkUnknownWrenchContacts_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1464: flag=_wrap_delete_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1465: flag=_wrap_new_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1466: flag=_wrap_estimateExternalWrenchesBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1467: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1468: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1469: flag=_wrap_estimateExternalWrenchesBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1470: flag=_wrap_estimateExternalWrenchesBuffers_A_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1471: flag=_wrap_estimateExternalWrenchesBuffers_A_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1472: flag=_wrap_estimateExternalWrenchesBuffers_x_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1473: flag=_wrap_estimateExternalWrenchesBuffers_x_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1474: flag=_wrap_estimateExternalWrenchesBuffers_b_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1475: flag=_wrap_estimateExternalWrenchesBuffers_b_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1476: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1477: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1478: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1479: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1480: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1481: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1482: flag=_wrap_delete_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1483: flag=_wrap_estimateExternalWrenchesWithoutInternalFT(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1484: flag=_wrap_estimateExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1485: flag=_wrap_dynamicsEstimationForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1486: flag=_wrap_dynamicsEstimationForwardVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1487: flag=_wrap_computeLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1488: flag=_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1489: flag=_wrap_new_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1490: flag=_wrap_delete_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1491: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1492: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1493: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1494: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1495: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1496: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_submodels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1497: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1498: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1499: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1500: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1501: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1502: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1503: flag=_wrap_new_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1504: flag=_wrap_delete_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1505: flag=_wrap_SimpleLeggedOdometry_setModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1506: flag=_wrap_SimpleLeggedOdometry_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1507: flag=_wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1508: flag=_wrap_SimpleLeggedOdometry_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1509: flag=_wrap_SimpleLeggedOdometry_updateKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1510: flag=_wrap_SimpleLeggedOdometry_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1511: flag=_wrap_SimpleLeggedOdometry_changeFixedFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1512: flag=_wrap_SimpleLeggedOdometry_getCurrentFixedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1513: flag=_wrap_SimpleLeggedOdometry_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1514: flag=_wrap_isLinkBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1515: flag=_wrap_isJointBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1516: flag=_wrap_isDOFBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1517: flag=_wrap_new_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1518: flag=_wrap_BerdyOptions_berdyVariant_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1519: flag=_wrap_BerdyOptions_berdyVariant_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1520: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1521: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1522: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1523: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1524: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1525: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1526: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1527: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1528: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1529: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1530: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1531: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1532: flag=_wrap_BerdyOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1533: flag=_wrap_BerdyOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1534: flag=_wrap_BerdyOptions_checkConsistency(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1535: flag=_wrap_delete_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1536: flag=_wrap_BerdySensor_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1537: flag=_wrap_BerdySensor_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1538: flag=_wrap_BerdySensor_id_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1539: flag=_wrap_BerdySensor_id_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1540: flag=_wrap_BerdySensor_range_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1541: flag=_wrap_BerdySensor_range_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1542: flag=_wrap_BerdySensor_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1543: flag=_wrap_BerdySensor_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1544: flag=_wrap_new_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1545: flag=_wrap_delete_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1546: flag=_wrap_BerdyDynamicVariable_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1547: flag=_wrap_BerdyDynamicVariable_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1548: flag=_wrap_BerdyDynamicVariable_id_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1549: flag=_wrap_BerdyDynamicVariable_id_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1550: flag=_wrap_BerdyDynamicVariable_range_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1551: flag=_wrap_BerdyDynamicVariable_range_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1552: flag=_wrap_BerdyDynamicVariable_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1553: flag=_wrap_BerdyDynamicVariable_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1554: flag=_wrap_new_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1555: flag=_wrap_delete_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1556: flag=_wrap_new_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1557: flag=_wrap_BerdyHelper_dynamicTraversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1558: flag=_wrap_BerdyHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1559: flag=_wrap_BerdyHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1560: flag=_wrap_BerdyHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1561: flag=_wrap_BerdyHelper_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1562: flag=_wrap_BerdyHelper_getOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1563: flag=_wrap_BerdyHelper_getNrOfDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1564: flag=_wrap_BerdyHelper_getNrOfDynamicEquations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1565: flag=_wrap_BerdyHelper_getNrOfSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1566: flag=_wrap_BerdyHelper_resizeAndZeroBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1567: flag=_wrap_BerdyHelper_getBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1568: flag=_wrap_BerdyHelper_getSensorsOrdering(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1569: flag=_wrap_BerdyHelper_getRangeSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1570: flag=_wrap_BerdyHelper_getRangeDOFSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1571: flag=_wrap_BerdyHelper_getRangeJointSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1572: flag=_wrap_BerdyHelper_getRangeLinkSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1573: flag=_wrap_BerdyHelper_getRangeLinkVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1574: flag=_wrap_BerdyHelper_getRangeJointVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1575: flag=_wrap_BerdyHelper_getRangeDOFVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1576: flag=_wrap_BerdyHelper_getDynamicVariablesOrdering(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1577: flag=_wrap_BerdyHelper_serializeDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1578: flag=_wrap_BerdyHelper_serializeSensorVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1579: flag=_wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1580: flag=_wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1581: flag=_wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1582: flag=_wrap_BerdyHelper_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1583: flag=_wrap_BerdyHelper_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1584: flag=_wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1585: flag=_wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1586: flag=_wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1587: flag=_wrap_delete_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1588: flag=_wrap_new_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1589: flag=_wrap_delete_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1590: flag=_wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1591: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1592: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1593: flag=_wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1594: flag=_wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1595: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1596: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1597: flag=_wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1598: flag=_wrap_BerdySparseMAPSolver_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1599: flag=_wrap_BerdySparseMAPSolver_initialize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1600: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1601: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1602: flag=_wrap_BerdySparseMAPSolver_doEstimate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1603: flag=_wrap_BerdySparseMAPSolver_getLastEstimate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1604: flag=_wrap_DynamicsRegressorParameter_category_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1605: flag=_wrap_DynamicsRegressorParameter_category_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1606: flag=_wrap_DynamicsRegressorParameter_elemIndex_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1607: flag=_wrap_DynamicsRegressorParameter_elemIndex_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1608: flag=_wrap_DynamicsRegressorParameter_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1609: flag=_wrap_DynamicsRegressorParameter_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1610: flag=_wrap_DynamicsRegressorParameter_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1611: flag=_wrap_DynamicsRegressorParameter_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1612: flag=_wrap_DynamicsRegressorParameter_ne(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1613: flag=_wrap_new_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1614: flag=_wrap_delete_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1615: flag=_wrap_DynamicsRegressorParametersList_parameters_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1616: flag=_wrap_DynamicsRegressorParametersList_parameters_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1617: flag=_wrap_DynamicsRegressorParametersList_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1618: flag=_wrap_DynamicsRegressorParametersList_addParam(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1619: flag=_wrap_DynamicsRegressorParametersList_addList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1620: flag=_wrap_DynamicsRegressorParametersList_findParam(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1621: flag=_wrap_DynamicsRegressorParametersList_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1622: flag=_wrap_new_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1623: flag=_wrap_delete_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1624: flag=_wrap_new_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1625: flag=_wrap_delete_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1626: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1627: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1628: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1629: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1630: flag=_wrap_DynamicsRegressorGenerator_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1631: flag=_wrap_DynamicsRegressorGenerator_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1632: flag=_wrap_DynamicsRegressorGenerator_getNrOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1633: flag=_wrap_DynamicsRegressorGenerator_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1634: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1635: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1636: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutput(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1637: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1638: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1639: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1640: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1641: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1642: flag=_wrap_DynamicsRegressorGenerator_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1643: flag=_wrap_DynamicsRegressorGenerator_getNrOfFakeLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1644: flag=_wrap_DynamicsRegressorGenerator_getBaseLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1645: flag=_wrap_DynamicsRegressorGenerator_getSensorsModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1646: flag=_wrap_DynamicsRegressorGenerator_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1647: flag=_wrap_DynamicsRegressorGenerator_getSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1648: flag=_wrap_DynamicsRegressorGenerator_setTorqueSensorMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1649: flag=_wrap_DynamicsRegressorGenerator_computeRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1650: flag=_wrap_DynamicsRegressorGenerator_getModelParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1651: flag=_wrap_DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1652: flag=_wrap_DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1653: flag=_wrap_DynamicsRegressorGenerator_generate_random_regressors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1654: flag=_wrap_new_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1655: flag=_wrap_delete_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1656: flag=_wrap_KinDynComputations_loadRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1657: flag=_wrap_KinDynComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1658: flag=_wrap_KinDynComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1659: flag=_wrap_KinDynComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1660: flag=_wrap_KinDynComputations_setFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1661: flag=_wrap_KinDynComputations_getFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1662: flag=_wrap_KinDynComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1663: flag=_wrap_KinDynComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1664: flag=_wrap_KinDynComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1665: flag=_wrap_KinDynComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1666: flag=_wrap_KinDynComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1667: flag=_wrap_KinDynComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1668: flag=_wrap_KinDynComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1669: flag=_wrap_KinDynComputations_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1670: flag=_wrap_KinDynComputations_getRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1671: flag=_wrap_KinDynComputations_getRelativeJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1672: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1673: flag=_wrap_KinDynComputations_setJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1674: flag=_wrap_KinDynComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1675: flag=_wrap_KinDynComputations_getRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1676: flag=_wrap_KinDynComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1677: flag=_wrap_KinDynComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1678: flag=_wrap_KinDynComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1679: flag=_wrap_KinDynComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1680: flag=_wrap_KinDynComputations_getModelVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1681: flag=_wrap_KinDynComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1682: flag=_wrap_KinDynComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1683: flag=_wrap_KinDynComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1684: flag=_wrap_KinDynComputations_getRelativeTransformExplicit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1685: flag=_wrap_KinDynComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1686: flag=_wrap_KinDynComputations_getFrameVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1687: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1688: flag=_wrap_KinDynComputations_getRelativeJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1689: flag=_wrap_KinDynComputations_getRelativeJacobianExplicit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1690: flag=_wrap_KinDynComputations_getFrameBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1691: flag=_wrap_KinDynComputations_getCenterOfMassPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1692: flag=_wrap_KinDynComputations_getCenterOfMassVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1693: flag=_wrap_KinDynComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1694: flag=_wrap_KinDynComputations_getCenterOfMassBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1695: flag=_wrap_KinDynComputations_getAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1696: flag=_wrap_KinDynComputations_getAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1697: flag=_wrap_KinDynComputations_getCentroidalAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1698: flag=_wrap_KinDynComputations_getCentroidalAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1699: flag=_wrap_KinDynComputations_getLinearAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1700: flag=_wrap_KinDynComputations_getLinearAngularMomentumJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1701: flag=_wrap_KinDynComputations_getCentroidalTotalMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1702: flag=_wrap_KinDynComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1703: flag=_wrap_KinDynComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1704: flag=_wrap_KinDynComputations_generalizedBiasForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1705: flag=_wrap_KinDynComputations_generalizedGravityForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1706: flag=_wrap_delete_ICamera(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1707: flag=_wrap_ICamera_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1708: flag=_wrap_ICamera_setTarget(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1709: flag=_wrap_ICamera_setUpVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1710: flag=_wrap_ColorViz_r_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1711: flag=_wrap_ColorViz_r_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1712: flag=_wrap_ColorViz_g_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1713: flag=_wrap_ColorViz_g_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1714: flag=_wrap_ColorViz_b_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1715: flag=_wrap_ColorViz_b_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1716: flag=_wrap_ColorViz_a_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1717: flag=_wrap_ColorViz_a_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1718: flag=_wrap_new_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1719: flag=_wrap_delete_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1720: flag=_wrap_delete_ILight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1721: flag=_wrap_ILight_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1722: flag=_wrap_ILight_setType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1723: flag=_wrap_ILight_getType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1724: flag=_wrap_ILight_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1725: flag=_wrap_ILight_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1726: flag=_wrap_ILight_setDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1727: flag=_wrap_ILight_getDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1728: flag=_wrap_ILight_setAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1729: flag=_wrap_ILight_getAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1730: flag=_wrap_ILight_setSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1731: flag=_wrap_ILight_getSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1732: flag=_wrap_ILight_setDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1733: flag=_wrap_ILight_getDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1734: flag=_wrap_delete_IEnvironment(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1735: flag=_wrap_IEnvironment_getElements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1736: flag=_wrap_IEnvironment_setElementVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1737: flag=_wrap_IEnvironment_setBackgroundColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1738: flag=_wrap_IEnvironment_setAmbientLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1739: flag=_wrap_IEnvironment_getLights(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1740: flag=_wrap_IEnvironment_addLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1741: flag=_wrap_IEnvironment_lightViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1742: flag=_wrap_IEnvironment_removeLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1743: flag=_wrap_delete_IJetsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1744: flag=_wrap_IJetsVisualization_setJetsFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1745: flag=_wrap_IJetsVisualization_getNrOfJets(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1746: flag=_wrap_IJetsVisualization_getJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1747: flag=_wrap_IJetsVisualization_setJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1748: flag=_wrap_IJetsVisualization_setJetColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1749: flag=_wrap_IJetsVisualization_setJetsDimensions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1750: flag=_wrap_IJetsVisualization_setJetsIntensity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1751: flag=_wrap_delete_IModelVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1752: flag=_wrap_IModelVisualization_setPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1753: flag=_wrap_IModelVisualization_setLinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1754: flag=_wrap_IModelVisualization_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1755: flag=_wrap_IModelVisualization_getInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1756: flag=_wrap_IModelVisualization_setModelVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1757: flag=_wrap_IModelVisualization_setModelColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1758: flag=_wrap_IModelVisualization_resetModelColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1759: flag=_wrap_IModelVisualization_getLinkNames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1760: flag=_wrap_IModelVisualization_setLinkVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1761: flag=_wrap_IModelVisualization_getFeatures(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1762: flag=_wrap_IModelVisualization_setFeatureVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1763: flag=_wrap_IModelVisualization_jets(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1764: flag=_wrap_VisualizerOptions_verbose_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1765: flag=_wrap_VisualizerOptions_verbose_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1766: flag=_wrap_VisualizerOptions_winWidth_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1767: flag=_wrap_VisualizerOptions_winWidth_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1768: flag=_wrap_VisualizerOptions_winHeight_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1769: flag=_wrap_VisualizerOptions_winHeight_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1770: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1771: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1772: flag=_wrap_new_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1773: flag=_wrap_delete_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1774: flag=_wrap_new_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1775: flag=_wrap_delete_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1776: flag=_wrap_Visualizer_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1777: flag=_wrap_Visualizer_getNrOfVisualizedModels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1778: flag=_wrap_Visualizer_getModelInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1779: flag=_wrap_Visualizer_getModelInstanceIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1780: flag=_wrap_Visualizer_addModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1781: flag=_wrap_Visualizer_modelViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1782: flag=_wrap_Visualizer_camera(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1783: flag=_wrap_Visualizer_enviroment(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1784: flag=_wrap_Visualizer_run(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1785: flag=_wrap_Visualizer_draw(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1786: flag=_wrap_Visualizer_drawToFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1787: flag=_wrap_Visualizer_close(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1788: flag=_wrap_new_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1789: flag=_wrap_delete_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1790: flag=_wrap_DynamicsComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1791: flag=_wrap_DynamicsComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1792: flag=_wrap_DynamicsComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1793: flag=_wrap_DynamicsComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1794: flag=_wrap_DynamicsComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1795: flag=_wrap_DynamicsComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1796: flag=_wrap_DynamicsComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1797: flag=_wrap_DynamicsComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1798: flag=_wrap_DynamicsComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1799: flag=_wrap_DynamicsComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1800: flag=_wrap_DynamicsComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1801: flag=_wrap_DynamicsComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1802: flag=_wrap_DynamicsComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1803: flag=_wrap_DynamicsComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1804: flag=_wrap_DynamicsComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1805: flag=_wrap_DynamicsComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1806: flag=_wrap_DynamicsComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1807: flag=_wrap_DynamicsComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1808: flag=_wrap_DynamicsComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1809: flag=_wrap_DynamicsComputations_getFrameTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1810: flag=_wrap_DynamicsComputations_getFrameTwistInWorldOrient(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1811: flag=_wrap_DynamicsComputations_getFrameProperSpatialAcceleration(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1812: flag=_wrap_DynamicsComputations_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1813: flag=_wrap_DynamicsComputations_getLinkInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1814: flag=_wrap_DynamicsComputations_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1815: flag=_wrap_DynamicsComputations_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1816: flag=_wrap_DynamicsComputations_getJointLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1817: flag=_wrap_DynamicsComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1818: flag=_wrap_DynamicsComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1819: flag=_wrap_DynamicsComputations_getFrameJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1820: flag=_wrap_DynamicsComputations_getDynamicsRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1821: flag=_wrap_DynamicsComputations_getModelDynamicsParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1822: flag=_wrap_DynamicsComputations_getCenterOfMass(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1823: flag=_wrap_DynamicsComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 816: flag=_wrap_dynamic_extent_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 817: flag=_wrap_DynamicSpan_extent_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 818: flag=_wrap_new_DynamicSpan(resc,resv,argc,(mxArray**)(argv)); break;
+  case 819: flag=_wrap_delete_DynamicSpan(resc,resv,argc,(mxArray**)(argv)); break;
+  case 820: flag=_wrap_DynamicSpan_first(resc,resv,argc,(mxArray**)(argv)); break;
+  case 821: flag=_wrap_DynamicSpan_last(resc,resv,argc,(mxArray**)(argv)); break;
+  case 822: flag=_wrap_DynamicSpan_subspan(resc,resv,argc,(mxArray**)(argv)); break;
+  case 823: flag=_wrap_DynamicSpan_size(resc,resv,argc,(mxArray**)(argv)); break;
+  case 824: flag=_wrap_DynamicSpan_size_bytes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 825: flag=_wrap_DynamicSpan_empty(resc,resv,argc,(mxArray**)(argv)); break;
+  case 826: flag=_wrap_DynamicSpan_brace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 827: flag=_wrap_DynamicSpan_getVal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 828: flag=_wrap_DynamicSpan_setVal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 829: flag=_wrap_DynamicSpan_at(resc,resv,argc,(mxArray**)(argv)); break;
+  case 830: flag=_wrap_DynamicSpan_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 831: flag=_wrap_DynamicSpan_begin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 832: flag=_wrap_DynamicSpan_end(resc,resv,argc,(mxArray**)(argv)); break;
+  case 833: flag=_wrap_DynamicSpan_cbegin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 834: flag=_wrap_DynamicSpan_cend(resc,resv,argc,(mxArray**)(argv)); break;
+  case 835: flag=_wrap_DynamicSpan_rbegin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 836: flag=_wrap_DynamicSpan_rend(resc,resv,argc,(mxArray**)(argv)); break;
+  case 837: flag=_wrap_DynamicSpan_crbegin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 838: flag=_wrap_DynamicSpan_crend(resc,resv,argc,(mxArray**)(argv)); break;
+  case 839: flag=_wrap_LINK_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 840: flag=_wrap_LINK_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 841: flag=_wrap_LINK_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 842: flag=_wrap_LINK_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 843: flag=_wrap_JOINT_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 844: flag=_wrap_JOINT_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 845: flag=_wrap_JOINT_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 846: flag=_wrap_JOINT_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 847: flag=_wrap_DOF_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 848: flag=_wrap_DOF_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 849: flag=_wrap_DOF_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 850: flag=_wrap_DOF_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 851: flag=_wrap_FRAME_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 852: flag=_wrap_FRAME_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 853: flag=_wrap_FRAME_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 854: flag=_wrap_FRAME_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 855: flag=_wrap_TRAVERSAL_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 856: flag=_wrap_TRAVERSAL_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 857: flag=_wrap_new_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 858: flag=_wrap_LinkPositions_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 859: flag=_wrap_LinkPositions_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 860: flag=_wrap_LinkPositions_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 861: flag=_wrap_LinkPositions_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 862: flag=_wrap_LinkPositions_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 863: flag=_wrap_delete_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 864: flag=_wrap_new_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 865: flag=_wrap_LinkWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 866: flag=_wrap_LinkWrenches_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 867: flag=_wrap_LinkWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 868: flag=_wrap_LinkWrenches_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 869: flag=_wrap_LinkWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 870: flag=_wrap_LinkWrenches_zero(resc,resv,argc,(mxArray**)(argv)); break;
+  case 871: flag=_wrap_delete_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 872: flag=_wrap_new_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 873: flag=_wrap_LinkInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 874: flag=_wrap_LinkInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 875: flag=_wrap_LinkInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 876: flag=_wrap_delete_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 877: flag=_wrap_new_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 878: flag=_wrap_LinkArticulatedBodyInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 879: flag=_wrap_LinkArticulatedBodyInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 880: flag=_wrap_LinkArticulatedBodyInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 881: flag=_wrap_delete_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 882: flag=_wrap_new_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 883: flag=_wrap_LinkVelArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 884: flag=_wrap_LinkVelArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 885: flag=_wrap_LinkVelArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 886: flag=_wrap_LinkVelArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 887: flag=_wrap_LinkVelArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 888: flag=_wrap_delete_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 889: flag=_wrap_new_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 890: flag=_wrap_LinkAccArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 891: flag=_wrap_LinkAccArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 892: flag=_wrap_LinkAccArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 893: flag=_wrap_LinkAccArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 894: flag=_wrap_LinkAccArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 895: flag=_wrap_delete_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 896: flag=_wrap_new_Link(resc,resv,argc,(mxArray**)(argv)); break;
+  case 897: flag=_wrap_Link_inertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 898: flag=_wrap_Link_setInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 899: flag=_wrap_Link_getInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 900: flag=_wrap_Link_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 901: flag=_wrap_Link_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 902: flag=_wrap_delete_Link(resc,resv,argc,(mxArray**)(argv)); break;
+  case 903: flag=_wrap_delete_IJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 904: flag=_wrap_IJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 905: flag=_wrap_IJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 906: flag=_wrap_IJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 907: flag=_wrap_IJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 908: flag=_wrap_IJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 909: flag=_wrap_IJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 910: flag=_wrap_IJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 911: flag=_wrap_IJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 912: flag=_wrap_IJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 913: flag=_wrap_IJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 914: flag=_wrap_IJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 915: flag=_wrap_IJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 916: flag=_wrap_IJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 917: flag=_wrap_IJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 918: flag=_wrap_IJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 919: flag=_wrap_IJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 920: flag=_wrap_IJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 921: flag=_wrap_IJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 922: flag=_wrap_IJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 923: flag=_wrap_IJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 924: flag=_wrap_IJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 925: flag=_wrap_IJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 926: flag=_wrap_IJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 927: flag=_wrap_IJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 928: flag=_wrap_IJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 929: flag=_wrap_IJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 930: flag=_wrap_IJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 931: flag=_wrap_IJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 932: flag=_wrap_IJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 933: flag=_wrap_IJoint_isRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 934: flag=_wrap_IJoint_isFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 935: flag=_wrap_IJoint_asRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 936: flag=_wrap_IJoint_asFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 937: flag=_wrap_new_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 938: flag=_wrap_delete_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 939: flag=_wrap_FixedJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 940: flag=_wrap_FixedJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 941: flag=_wrap_FixedJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 942: flag=_wrap_FixedJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 943: flag=_wrap_FixedJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 944: flag=_wrap_FixedJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 945: flag=_wrap_FixedJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 946: flag=_wrap_FixedJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 947: flag=_wrap_FixedJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 948: flag=_wrap_FixedJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 949: flag=_wrap_FixedJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 950: flag=_wrap_FixedJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 951: flag=_wrap_FixedJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 952: flag=_wrap_FixedJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 953: flag=_wrap_FixedJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 954: flag=_wrap_FixedJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 955: flag=_wrap_FixedJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 956: flag=_wrap_FixedJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 957: flag=_wrap_FixedJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 958: flag=_wrap_FixedJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 959: flag=_wrap_FixedJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 960: flag=_wrap_FixedJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 961: flag=_wrap_FixedJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 962: flag=_wrap_FixedJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 963: flag=_wrap_FixedJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 964: flag=_wrap_FixedJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 965: flag=_wrap_FixedJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 966: flag=_wrap_FixedJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 967: flag=_wrap_FixedJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 968: flag=_wrap_delete_MovableJointImpl1(resc,resv,argc,(mxArray**)(argv)); break;
+  case 969: flag=_wrap_MovableJointImpl1_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 970: flag=_wrap_MovableJointImpl1_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 971: flag=_wrap_MovableJointImpl1_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 972: flag=_wrap_MovableJointImpl1_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 973: flag=_wrap_MovableJointImpl1_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 974: flag=_wrap_MovableJointImpl1_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 975: flag=_wrap_MovableJointImpl1_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 976: flag=_wrap_MovableJointImpl1_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 977: flag=_wrap_delete_MovableJointImpl2(resc,resv,argc,(mxArray**)(argv)); break;
+  case 978: flag=_wrap_MovableJointImpl2_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 979: flag=_wrap_MovableJointImpl2_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 980: flag=_wrap_MovableJointImpl2_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 981: flag=_wrap_MovableJointImpl2_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 982: flag=_wrap_MovableJointImpl2_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 983: flag=_wrap_MovableJointImpl2_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 984: flag=_wrap_MovableJointImpl2_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 985: flag=_wrap_MovableJointImpl2_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 986: flag=_wrap_delete_MovableJointImpl3(resc,resv,argc,(mxArray**)(argv)); break;
+  case 987: flag=_wrap_MovableJointImpl3_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 988: flag=_wrap_MovableJointImpl3_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 989: flag=_wrap_MovableJointImpl3_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 990: flag=_wrap_MovableJointImpl3_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 991: flag=_wrap_MovableJointImpl3_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 992: flag=_wrap_MovableJointImpl3_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 993: flag=_wrap_MovableJointImpl3_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 994: flag=_wrap_MovableJointImpl3_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 995: flag=_wrap_delete_MovableJointImpl4(resc,resv,argc,(mxArray**)(argv)); break;
+  case 996: flag=_wrap_MovableJointImpl4_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 997: flag=_wrap_MovableJointImpl4_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 998: flag=_wrap_MovableJointImpl4_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 999: flag=_wrap_MovableJointImpl4_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1000: flag=_wrap_MovableJointImpl4_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1001: flag=_wrap_MovableJointImpl4_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1002: flag=_wrap_MovableJointImpl4_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1003: flag=_wrap_MovableJointImpl4_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1004: flag=_wrap_delete_MovableJointImpl5(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1005: flag=_wrap_MovableJointImpl5_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1006: flag=_wrap_MovableJointImpl5_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1007: flag=_wrap_MovableJointImpl5_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1008: flag=_wrap_MovableJointImpl5_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1009: flag=_wrap_MovableJointImpl5_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1010: flag=_wrap_MovableJointImpl5_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1011: flag=_wrap_MovableJointImpl5_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1012: flag=_wrap_MovableJointImpl5_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1013: flag=_wrap_delete_MovableJointImpl6(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1014: flag=_wrap_MovableJointImpl6_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1015: flag=_wrap_MovableJointImpl6_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1016: flag=_wrap_MovableJointImpl6_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1017: flag=_wrap_MovableJointImpl6_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1018: flag=_wrap_MovableJointImpl6_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1019: flag=_wrap_MovableJointImpl6_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1020: flag=_wrap_MovableJointImpl6_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1021: flag=_wrap_MovableJointImpl6_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1022: flag=_wrap_new_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1023: flag=_wrap_delete_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1024: flag=_wrap_RevoluteJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1025: flag=_wrap_RevoluteJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1026: flag=_wrap_RevoluteJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1027: flag=_wrap_RevoluteJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1028: flag=_wrap_RevoluteJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1029: flag=_wrap_RevoluteJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1030: flag=_wrap_RevoluteJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1031: flag=_wrap_RevoluteJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1032: flag=_wrap_RevoluteJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1033: flag=_wrap_RevoluteJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1034: flag=_wrap_RevoluteJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1035: flag=_wrap_RevoluteJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1036: flag=_wrap_RevoluteJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1037: flag=_wrap_RevoluteJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1038: flag=_wrap_RevoluteJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1039: flag=_wrap_RevoluteJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1040: flag=_wrap_RevoluteJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1041: flag=_wrap_RevoluteJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1042: flag=_wrap_RevoluteJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1043: flag=_wrap_RevoluteJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1044: flag=_wrap_RevoluteJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1045: flag=_wrap_RevoluteJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1046: flag=_wrap_RevoluteJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1047: flag=_wrap_new_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1048: flag=_wrap_delete_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1049: flag=_wrap_PrismaticJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1050: flag=_wrap_PrismaticJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1051: flag=_wrap_PrismaticJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1052: flag=_wrap_PrismaticJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1053: flag=_wrap_PrismaticJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1054: flag=_wrap_PrismaticJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1055: flag=_wrap_PrismaticJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1056: flag=_wrap_PrismaticJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1057: flag=_wrap_PrismaticJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1058: flag=_wrap_PrismaticJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1059: flag=_wrap_PrismaticJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1060: flag=_wrap_PrismaticJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1061: flag=_wrap_PrismaticJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1062: flag=_wrap_PrismaticJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1063: flag=_wrap_PrismaticJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1064: flag=_wrap_PrismaticJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1065: flag=_wrap_PrismaticJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1066: flag=_wrap_PrismaticJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1067: flag=_wrap_PrismaticJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1068: flag=_wrap_PrismaticJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1069: flag=_wrap_PrismaticJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1070: flag=_wrap_PrismaticJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1071: flag=_wrap_PrismaticJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1072: flag=_wrap_new_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1073: flag=_wrap_delete_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1074: flag=_wrap_Traversal_getNrOfVisitedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1075: flag=_wrap_Traversal_getLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1076: flag=_wrap_Traversal_getBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1077: flag=_wrap_Traversal_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1078: flag=_wrap_Traversal_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1079: flag=_wrap_Traversal_getParentLinkFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1080: flag=_wrap_Traversal_getParentJointFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1081: flag=_wrap_Traversal_getTraversalIndexFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1082: flag=_wrap_Traversal_reset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1083: flag=_wrap_Traversal_addTraversalBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1084: flag=_wrap_Traversal_addTraversalElement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1085: flag=_wrap_Traversal_isParentOf(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1086: flag=_wrap_Traversal_getChildLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1087: flag=_wrap_Traversal_getParentLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1088: flag=_wrap_Traversal_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1089: flag=_wrap_delete_SolidShape(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1090: flag=_wrap_SolidShape_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1091: flag=_wrap_SolidShape_name_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1092: flag=_wrap_SolidShape_name_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1093: flag=_wrap_SolidShape_link_H_geometry_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1094: flag=_wrap_SolidShape_link_H_geometry_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1095: flag=_wrap_SolidShape_material_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1096: flag=_wrap_SolidShape_material_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1097: flag=_wrap_SolidShape_isSphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1098: flag=_wrap_SolidShape_isBox(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1099: flag=_wrap_SolidShape_isCylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1100: flag=_wrap_SolidShape_isExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1101: flag=_wrap_SolidShape_asSphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1102: flag=_wrap_SolidShape_asBox(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1103: flag=_wrap_SolidShape_asCylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1104: flag=_wrap_SolidShape_asExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1105: flag=_wrap_delete_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1106: flag=_wrap_Sphere_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1107: flag=_wrap_Sphere_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1108: flag=_wrap_Sphere_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1109: flag=_wrap_new_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1110: flag=_wrap_delete_Box(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1111: flag=_wrap_Box_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1112: flag=_wrap_Box_x_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1113: flag=_wrap_Box_x_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1114: flag=_wrap_Box_y_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1115: flag=_wrap_Box_y_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1116: flag=_wrap_Box_z_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1117: flag=_wrap_Box_z_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1118: flag=_wrap_new_Box(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1119: flag=_wrap_delete_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1120: flag=_wrap_Cylinder_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1121: flag=_wrap_Cylinder_length_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1122: flag=_wrap_Cylinder_length_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1123: flag=_wrap_Cylinder_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1124: flag=_wrap_Cylinder_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1125: flag=_wrap_new_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1126: flag=_wrap_delete_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1127: flag=_wrap_ExternalMesh_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1128: flag=_wrap_ExternalMesh_filename_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1129: flag=_wrap_ExternalMesh_filename_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1130: flag=_wrap_ExternalMesh_scale_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1131: flag=_wrap_ExternalMesh_scale_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1132: flag=_wrap_new_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1133: flag=_wrap_new_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1134: flag=_wrap_ModelSolidShapes_clear(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1135: flag=_wrap_delete_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1136: flag=_wrap_ModelSolidShapes_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1137: flag=_wrap_ModelSolidShapes_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1138: flag=_wrap_ModelSolidShapes_linkSolidShapes_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1139: flag=_wrap_ModelSolidShapes_linkSolidShapes_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1140: flag=_wrap_Neighbor_neighborLink_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1141: flag=_wrap_Neighbor_neighborLink_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1142: flag=_wrap_Neighbor_neighborJoint_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1143: flag=_wrap_Neighbor_neighborJoint_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1144: flag=_wrap_new_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1145: flag=_wrap_delete_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1146: flag=_wrap_new_Model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1147: flag=_wrap_Model_copy(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1148: flag=_wrap_delete_Model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1149: flag=_wrap_Model_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1150: flag=_wrap_Model_getLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1151: flag=_wrap_Model_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1152: flag=_wrap_Model_isValidLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1153: flag=_wrap_Model_getLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1154: flag=_wrap_Model_addLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1155: flag=_wrap_Model_getNrOfJoints(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1156: flag=_wrap_Model_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1157: flag=_wrap_Model_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1158: flag=_wrap_Model_getJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1159: flag=_wrap_Model_isValidJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1160: flag=_wrap_Model_isLinkNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1161: flag=_wrap_Model_isJointNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1162: flag=_wrap_Model_isFrameNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1163: flag=_wrap_Model_addJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1164: flag=_wrap_Model_addJointAndLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1165: flag=_wrap_Model_insertLinkToExistingJointAndAddJointForDisplacedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1166: flag=_wrap_Model_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1167: flag=_wrap_Model_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1168: flag=_wrap_Model_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1169: flag=_wrap_Model_addAdditionalFrameToLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1170: flag=_wrap_Model_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1171: flag=_wrap_Model_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1172: flag=_wrap_Model_isValidFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1173: flag=_wrap_Model_getFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1174: flag=_wrap_Model_getFrameLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1175: flag=_wrap_Model_getNrOfNeighbors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1176: flag=_wrap_Model_getNeighbor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1177: flag=_wrap_Model_setDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1178: flag=_wrap_Model_getDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1179: flag=_wrap_Model_computeFullTreeTraversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1180: flag=_wrap_Model_getInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1181: flag=_wrap_Model_updateInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1182: flag=_wrap_Model_visualSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1183: flag=_wrap_Model_collisionSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1184: flag=_wrap_Model_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1185: flag=_wrap_new_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1186: flag=_wrap_JointPosDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1187: flag=_wrap_JointPosDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1188: flag=_wrap_delete_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1189: flag=_wrap_new_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1190: flag=_wrap_JointDOFsDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1191: flag=_wrap_JointDOFsDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1192: flag=_wrap_delete_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1193: flag=_wrap_new_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1194: flag=_wrap_DOFSpatialForceArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1195: flag=_wrap_DOFSpatialForceArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1196: flag=_wrap_DOFSpatialForceArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1197: flag=_wrap_delete_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1198: flag=_wrap_new_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1199: flag=_wrap_DOFSpatialMotionArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1200: flag=_wrap_DOFSpatialMotionArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1201: flag=_wrap_DOFSpatialMotionArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1202: flag=_wrap_delete_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1203: flag=_wrap_new_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1204: flag=_wrap_FrameFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1205: flag=_wrap_FrameFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1206: flag=_wrap_delete_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1207: flag=_wrap_new_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1208: flag=_wrap_MomentumFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1209: flag=_wrap_MomentumFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1210: flag=_wrap_delete_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1211: flag=_wrap_new_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1212: flag=_wrap_FreeFloatingMassMatrix_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1213: flag=_wrap_delete_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1214: flag=_wrap_new_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1215: flag=_wrap_FreeFloatingPos_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1216: flag=_wrap_FreeFloatingPos_worldBasePos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1217: flag=_wrap_FreeFloatingPos_jointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1218: flag=_wrap_FreeFloatingPos_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1219: flag=_wrap_delete_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1220: flag=_wrap_new_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1221: flag=_wrap_FreeFloatingGeneralizedTorques_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1222: flag=_wrap_FreeFloatingGeneralizedTorques_baseWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1223: flag=_wrap_FreeFloatingGeneralizedTorques_jointTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1224: flag=_wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1225: flag=_wrap_delete_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1226: flag=_wrap_new_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1227: flag=_wrap_FreeFloatingVel_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1228: flag=_wrap_FreeFloatingVel_baseVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1229: flag=_wrap_FreeFloatingVel_jointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1230: flag=_wrap_FreeFloatingVel_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1231: flag=_wrap_delete_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1232: flag=_wrap_new_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1233: flag=_wrap_FreeFloatingAcc_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1234: flag=_wrap_FreeFloatingAcc_baseAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1235: flag=_wrap_FreeFloatingAcc_jointAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1236: flag=_wrap_FreeFloatingAcc_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1237: flag=_wrap_delete_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1238: flag=_wrap_ContactWrench_contactId(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1239: flag=_wrap_ContactWrench_contactPoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1240: flag=_wrap_ContactWrench_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1241: flag=_wrap_new_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1242: flag=_wrap_delete_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1243: flag=_wrap_new_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1244: flag=_wrap_LinkContactWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1245: flag=_wrap_LinkContactWrenches_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1246: flag=_wrap_LinkContactWrenches_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1247: flag=_wrap_LinkContactWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1248: flag=_wrap_LinkContactWrenches_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1249: flag=_wrap_LinkContactWrenches_computeNetWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1250: flag=_wrap_LinkContactWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1251: flag=_wrap_delete_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1252: flag=_wrap_ForwardPositionKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1253: flag=_wrap_ForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1254: flag=_wrap_ForwardPosVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1255: flag=_wrap_ForwardPosVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1256: flag=_wrap_ForwardAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1257: flag=_wrap_ForwardBiasAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1258: flag=_wrap_ComputeLinearAndAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1259: flag=_wrap_ComputeLinearAndAngularMomentumDerivativeBias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1260: flag=_wrap_RNEADynamicPhase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1261: flag=_wrap_CompositeRigidBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1262: flag=_wrap_new_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1263: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1264: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1265: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1266: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1267: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1268: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1269: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1270: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1271: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1272: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1273: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1274: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1275: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1276: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1277: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1278: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1279: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1280: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1281: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1282: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1283: flag=_wrap_delete_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1284: flag=_wrap_ArticulatedBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1285: flag=_wrap_InverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1286: flag=_wrap_NR_OF_SENSOR_TYPES_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1287: flag=_wrap_isLinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1288: flag=_wrap_isJointSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1289: flag=_wrap_getSensorTypeSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1290: flag=_wrap_delete_Sensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1291: flag=_wrap_Sensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1292: flag=_wrap_Sensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1293: flag=_wrap_Sensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1294: flag=_wrap_Sensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1295: flag=_wrap_Sensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1296: flag=_wrap_Sensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1297: flag=_wrap_Sensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1298: flag=_wrap_Sensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1299: flag=_wrap_delete_JointSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1300: flag=_wrap_JointSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1301: flag=_wrap_JointSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1302: flag=_wrap_JointSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1303: flag=_wrap_JointSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1304: flag=_wrap_JointSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1305: flag=_wrap_delete_LinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1306: flag=_wrap_LinkSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1307: flag=_wrap_LinkSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1308: flag=_wrap_LinkSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1309: flag=_wrap_LinkSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1310: flag=_wrap_LinkSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1311: flag=_wrap_LinkSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1312: flag=_wrap_LinkSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1313: flag=_wrap_new_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1314: flag=_wrap_delete_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1315: flag=_wrap_SensorsList_addSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1316: flag=_wrap_SensorsList_setSerialization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1317: flag=_wrap_SensorsList_getSerialization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1318: flag=_wrap_SensorsList_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1319: flag=_wrap_SensorsList_getSensorIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1320: flag=_wrap_SensorsList_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1321: flag=_wrap_SensorsList_getSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1322: flag=_wrap_SensorsList_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1323: flag=_wrap_SensorsList_removeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1324: flag=_wrap_SensorsList_removeAllSensorsOfType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1325: flag=_wrap_SensorsList_getSixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1326: flag=_wrap_SensorsList_getAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1327: flag=_wrap_SensorsList_getGyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1328: flag=_wrap_SensorsList_getThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1329: flag=_wrap_SensorsList_getThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1330: flag=_wrap_new_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1331: flag=_wrap_delete_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1332: flag=_wrap_SensorsMeasurements_setNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1333: flag=_wrap_SensorsMeasurements_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1334: flag=_wrap_SensorsMeasurements_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1335: flag=_wrap_SensorsMeasurements_toVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1336: flag=_wrap_SensorsMeasurements_setMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1337: flag=_wrap_SensorsMeasurements_getMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1338: flag=_wrap_SensorsMeasurements_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1339: flag=_wrap_new_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1340: flag=_wrap_delete_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1341: flag=_wrap_SixAxisForceTorqueSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1342: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1343: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1344: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1345: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1346: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1347: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1348: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1349: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1350: flag=_wrap_SixAxisForceTorqueSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1351: flag=_wrap_SixAxisForceTorqueSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1352: flag=_wrap_SixAxisForceTorqueSensor_setAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1353: flag=_wrap_SixAxisForceTorqueSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1354: flag=_wrap_SixAxisForceTorqueSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1355: flag=_wrap_SixAxisForceTorqueSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1356: flag=_wrap_SixAxisForceTorqueSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1357: flag=_wrap_SixAxisForceTorqueSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1358: flag=_wrap_SixAxisForceTorqueSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1359: flag=_wrap_SixAxisForceTorqueSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1360: flag=_wrap_SixAxisForceTorqueSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1361: flag=_wrap_SixAxisForceTorqueSensor_getAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1362: flag=_wrap_SixAxisForceTorqueSensor_isLinkAttachedToSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1363: flag=_wrap_SixAxisForceTorqueSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1364: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1365: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1366: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1367: flag=_wrap_SixAxisForceTorqueSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1368: flag=_wrap_SixAxisForceTorqueSensor_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1369: flag=_wrap_new_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1370: flag=_wrap_delete_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1371: flag=_wrap_AccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1372: flag=_wrap_AccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1373: flag=_wrap_AccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1374: flag=_wrap_AccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1375: flag=_wrap_AccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1376: flag=_wrap_AccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1377: flag=_wrap_AccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1378: flag=_wrap_AccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1379: flag=_wrap_AccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1380: flag=_wrap_AccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1381: flag=_wrap_AccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1382: flag=_wrap_AccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1383: flag=_wrap_AccelerometerSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1384: flag=_wrap_AccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1385: flag=_wrap_new_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1386: flag=_wrap_delete_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1387: flag=_wrap_GyroscopeSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1388: flag=_wrap_GyroscopeSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1389: flag=_wrap_GyroscopeSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1390: flag=_wrap_GyroscopeSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1391: flag=_wrap_GyroscopeSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1392: flag=_wrap_GyroscopeSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1393: flag=_wrap_GyroscopeSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1394: flag=_wrap_GyroscopeSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1395: flag=_wrap_GyroscopeSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1396: flag=_wrap_GyroscopeSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1397: flag=_wrap_GyroscopeSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1398: flag=_wrap_GyroscopeSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1399: flag=_wrap_GyroscopeSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1400: flag=_wrap_GyroscopeSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1401: flag=_wrap_new_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1402: flag=_wrap_delete_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1403: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1404: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1405: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1406: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1407: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1408: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1409: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1410: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1411: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1412: flag=_wrap_ThreeAxisAngularAccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1413: flag=_wrap_ThreeAxisAngularAccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1414: flag=_wrap_ThreeAxisAngularAccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1415: flag=_wrap_ThreeAxisAngularAccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1416: flag=_wrap_new_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1417: flag=_wrap_delete_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1418: flag=_wrap_ThreeAxisForceTorqueContactSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1419: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1420: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1421: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1422: flag=_wrap_ThreeAxisForceTorqueContactSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1423: flag=_wrap_ThreeAxisForceTorqueContactSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1424: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1425: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1426: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1427: flag=_wrap_ThreeAxisForceTorqueContactSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1428: flag=_wrap_ThreeAxisForceTorqueContactSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1429: flag=_wrap_ThreeAxisForceTorqueContactSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1430: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1431: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1432: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1433: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1434: flag=_wrap_predictSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1435: flag=_wrap_predictSensorsMeasurementsFromRawBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1436: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1437: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1438: flag=_wrap_URDFParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1439: flag=_wrap_URDFParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1440: flag=_wrap_new_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1441: flag=_wrap_delete_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1442: flag=_wrap_modelFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1443: flag=_wrap_modelFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1444: flag=_wrap_dofsListFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1445: flag=_wrap_dofsListFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1446: flag=_wrap_sensorsFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1447: flag=_wrap_sensorsFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1448: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1449: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1450: flag=_wrap_ModelParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1451: flag=_wrap_ModelParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1452: flag=_wrap_new_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1453: flag=_wrap_delete_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1454: flag=_wrap_new_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1455: flag=_wrap_delete_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1456: flag=_wrap_ModelLoader_parsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1457: flag=_wrap_ModelLoader_setParsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1458: flag=_wrap_ModelLoader_loadModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1459: flag=_wrap_ModelLoader_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1460: flag=_wrap_ModelLoader_loadReducedModelFromFullModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1461: flag=_wrap_ModelLoader_loadReducedModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1462: flag=_wrap_ModelLoader_loadReducedModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1463: flag=_wrap_ModelLoader_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1464: flag=_wrap_ModelLoader_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1465: flag=_wrap_ModelLoader_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1466: flag=_wrap_new_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1467: flag=_wrap_UnknownWrenchContact_unknownType_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1468: flag=_wrap_UnknownWrenchContact_unknownType_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1469: flag=_wrap_UnknownWrenchContact_contactPoint_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1470: flag=_wrap_UnknownWrenchContact_contactPoint_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1471: flag=_wrap_UnknownWrenchContact_forceDirection_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1472: flag=_wrap_UnknownWrenchContact_forceDirection_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1473: flag=_wrap_UnknownWrenchContact_knownWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1474: flag=_wrap_UnknownWrenchContact_knownWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1475: flag=_wrap_UnknownWrenchContact_contactId_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1476: flag=_wrap_UnknownWrenchContact_contactId_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1477: flag=_wrap_delete_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1478: flag=_wrap_new_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1479: flag=_wrap_LinkUnknownWrenchContacts_clear(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1480: flag=_wrap_LinkUnknownWrenchContacts_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1481: flag=_wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1482: flag=_wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1483: flag=_wrap_LinkUnknownWrenchContacts_addNewContactForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1484: flag=_wrap_LinkUnknownWrenchContacts_addNewContactInFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1485: flag=_wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1486: flag=_wrap_LinkUnknownWrenchContacts_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1487: flag=_wrap_LinkUnknownWrenchContacts_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1488: flag=_wrap_delete_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1489: flag=_wrap_new_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1490: flag=_wrap_estimateExternalWrenchesBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1491: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1492: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1493: flag=_wrap_estimateExternalWrenchesBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1494: flag=_wrap_estimateExternalWrenchesBuffers_A_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1495: flag=_wrap_estimateExternalWrenchesBuffers_A_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1496: flag=_wrap_estimateExternalWrenchesBuffers_x_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1497: flag=_wrap_estimateExternalWrenchesBuffers_x_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1498: flag=_wrap_estimateExternalWrenchesBuffers_b_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1499: flag=_wrap_estimateExternalWrenchesBuffers_b_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1500: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1501: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1502: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1503: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1504: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1505: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1506: flag=_wrap_delete_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1507: flag=_wrap_estimateExternalWrenchesWithoutInternalFT(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1508: flag=_wrap_estimateExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1509: flag=_wrap_dynamicsEstimationForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1510: flag=_wrap_dynamicsEstimationForwardVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1511: flag=_wrap_computeLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1512: flag=_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1513: flag=_wrap_new_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1514: flag=_wrap_delete_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1515: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1516: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1517: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1518: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1519: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1520: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_submodels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1521: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1522: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1523: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1524: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1525: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1526: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1527: flag=_wrap_new_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1528: flag=_wrap_delete_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1529: flag=_wrap_SimpleLeggedOdometry_setModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1530: flag=_wrap_SimpleLeggedOdometry_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1531: flag=_wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1532: flag=_wrap_SimpleLeggedOdometry_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1533: flag=_wrap_SimpleLeggedOdometry_updateKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1534: flag=_wrap_SimpleLeggedOdometry_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1535: flag=_wrap_SimpleLeggedOdometry_changeFixedFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1536: flag=_wrap_SimpleLeggedOdometry_getCurrentFixedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1537: flag=_wrap_SimpleLeggedOdometry_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1538: flag=_wrap_isLinkBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1539: flag=_wrap_isJointBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1540: flag=_wrap_isDOFBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1541: flag=_wrap_new_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1542: flag=_wrap_BerdyOptions_berdyVariant_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1543: flag=_wrap_BerdyOptions_berdyVariant_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1544: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1545: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1546: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1547: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1548: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1549: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1550: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1551: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1552: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1553: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1554: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1555: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1556: flag=_wrap_BerdyOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1557: flag=_wrap_BerdyOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1558: flag=_wrap_BerdyOptions_checkConsistency(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1559: flag=_wrap_delete_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1560: flag=_wrap_BerdySensor_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1561: flag=_wrap_BerdySensor_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1562: flag=_wrap_BerdySensor_id_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1563: flag=_wrap_BerdySensor_id_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1564: flag=_wrap_BerdySensor_range_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1565: flag=_wrap_BerdySensor_range_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1566: flag=_wrap_BerdySensor_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1567: flag=_wrap_BerdySensor_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1568: flag=_wrap_new_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1569: flag=_wrap_delete_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1570: flag=_wrap_BerdyDynamicVariable_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1571: flag=_wrap_BerdyDynamicVariable_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1572: flag=_wrap_BerdyDynamicVariable_id_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1573: flag=_wrap_BerdyDynamicVariable_id_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1574: flag=_wrap_BerdyDynamicVariable_range_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1575: flag=_wrap_BerdyDynamicVariable_range_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1576: flag=_wrap_BerdyDynamicVariable_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1577: flag=_wrap_BerdyDynamicVariable_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1578: flag=_wrap_new_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1579: flag=_wrap_delete_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1580: flag=_wrap_new_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1581: flag=_wrap_BerdyHelper_dynamicTraversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1582: flag=_wrap_BerdyHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1583: flag=_wrap_BerdyHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1584: flag=_wrap_BerdyHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1585: flag=_wrap_BerdyHelper_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1586: flag=_wrap_BerdyHelper_getOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1587: flag=_wrap_BerdyHelper_getNrOfDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1588: flag=_wrap_BerdyHelper_getNrOfDynamicEquations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1589: flag=_wrap_BerdyHelper_getNrOfSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1590: flag=_wrap_BerdyHelper_resizeAndZeroBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1591: flag=_wrap_BerdyHelper_getBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1592: flag=_wrap_BerdyHelper_getSensorsOrdering(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1593: flag=_wrap_BerdyHelper_getRangeSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1594: flag=_wrap_BerdyHelper_getRangeDOFSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1595: flag=_wrap_BerdyHelper_getRangeJointSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1596: flag=_wrap_BerdyHelper_getRangeLinkSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1597: flag=_wrap_BerdyHelper_getRangeLinkVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1598: flag=_wrap_BerdyHelper_getRangeJointVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1599: flag=_wrap_BerdyHelper_getRangeDOFVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1600: flag=_wrap_BerdyHelper_getDynamicVariablesOrdering(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1601: flag=_wrap_BerdyHelper_serializeDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1602: flag=_wrap_BerdyHelper_serializeSensorVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1603: flag=_wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1604: flag=_wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1605: flag=_wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1606: flag=_wrap_BerdyHelper_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1607: flag=_wrap_BerdyHelper_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1608: flag=_wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1609: flag=_wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1610: flag=_wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1611: flag=_wrap_delete_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1612: flag=_wrap_new_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1613: flag=_wrap_delete_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1614: flag=_wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1615: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1616: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1617: flag=_wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1618: flag=_wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1619: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1620: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1621: flag=_wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1622: flag=_wrap_BerdySparseMAPSolver_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1623: flag=_wrap_BerdySparseMAPSolver_initialize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1624: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1625: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1626: flag=_wrap_BerdySparseMAPSolver_doEstimate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1627: flag=_wrap_BerdySparseMAPSolver_getLastEstimate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1628: flag=_wrap_delete_IAttitudeEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1629: flag=_wrap_IAttitudeEstimator_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1630: flag=_wrap_IAttitudeEstimator_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1631: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1632: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1633: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1634: flag=_wrap_IAttitudeEstimator_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1635: flag=_wrap_IAttitudeEstimator_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1636: flag=_wrap_IAttitudeEstimator_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1637: flag=_wrap_IAttitudeEstimator_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1638: flag=_wrap_IAttitudeEstimator_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1639: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1640: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1641: flag=_wrap_AttitudeMahonyFilterParameters_kp_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1642: flag=_wrap_AttitudeMahonyFilterParameters_kp_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1643: flag=_wrap_AttitudeMahonyFilterParameters_ki_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1644: flag=_wrap_AttitudeMahonyFilterParameters_ki_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1645: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1646: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1647: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1648: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1649: flag=_wrap_new_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1650: flag=_wrap_delete_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1651: flag=_wrap_new_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1652: flag=_wrap_AttitudeMahonyFilter_useMagnetoMeterMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1653: flag=_wrap_AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1654: flag=_wrap_AttitudeMahonyFilter_setGainkp(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1655: flag=_wrap_AttitudeMahonyFilter_setGainki(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1656: flag=_wrap_AttitudeMahonyFilter_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1657: flag=_wrap_AttitudeMahonyFilter_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1658: flag=_wrap_AttitudeMahonyFilter_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1659: flag=_wrap_AttitudeMahonyFilter_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1660: flag=_wrap_AttitudeMahonyFilter_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1661: flag=_wrap_AttitudeMahonyFilter_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1662: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1663: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1664: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1665: flag=_wrap_AttitudeMahonyFilter_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1666: flag=_wrap_AttitudeMahonyFilter_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1667: flag=_wrap_AttitudeMahonyFilter_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1668: flag=_wrap_AttitudeMahonyFilter_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1669: flag=_wrap_AttitudeMahonyFilter_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1670: flag=_wrap_delete_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1671: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_f(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1672: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1673: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1674: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1675: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfPredict(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1676: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1677: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1678: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1679: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1680: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1681: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1682: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1683: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1684: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1685: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1686: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1687: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1688: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1689: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1690: flag=_wrap_delete_DiscreteExtendedKalmanFilterHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1691: flag=_wrap_output_dimensions_with_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1692: flag=_wrap_output_dimensions_without_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1693: flag=_wrap_input_dimensions_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1694: flag=_wrap_AttitudeEstimatorState_m_orientation_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1695: flag=_wrap_AttitudeEstimatorState_m_orientation_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1696: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1697: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1698: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1699: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1700: flag=_wrap_new_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1701: flag=_wrap_delete_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1702: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1703: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1704: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1705: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1706: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1707: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1708: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1709: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1710: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1711: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1712: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1713: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1714: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1715: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1716: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1717: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1718: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1719: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1720: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1721: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1722: flag=_wrap_new_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1723: flag=_wrap_delete_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1724: flag=_wrap_new_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1725: flag=_wrap_AttitudeQuaternionEKF_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1726: flag=_wrap_AttitudeQuaternionEKF_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1727: flag=_wrap_AttitudeQuaternionEKF_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1728: flag=_wrap_AttitudeQuaternionEKF_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1729: flag=_wrap_AttitudeQuaternionEKF_setBiasCorrelationTimeFactor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1730: flag=_wrap_AttitudeQuaternionEKF_useMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1731: flag=_wrap_AttitudeQuaternionEKF_setMeasurementNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1732: flag=_wrap_AttitudeQuaternionEKF_setSystemNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1733: flag=_wrap_AttitudeQuaternionEKF_setInitialStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1734: flag=_wrap_AttitudeQuaternionEKF_initializeFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1735: flag=_wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1736: flag=_wrap_AttitudeQuaternionEKF_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1737: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1738: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1739: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1740: flag=_wrap_AttitudeQuaternionEKF_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1741: flag=_wrap_AttitudeQuaternionEKF_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1742: flag=_wrap_AttitudeQuaternionEKF_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1743: flag=_wrap_AttitudeQuaternionEKF_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1744: flag=_wrap_AttitudeQuaternionEKF_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1745: flag=_wrap_delete_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1746: flag=_wrap_DynamicsRegressorParameter_category_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1747: flag=_wrap_DynamicsRegressorParameter_category_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1748: flag=_wrap_DynamicsRegressorParameter_elemIndex_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1749: flag=_wrap_DynamicsRegressorParameter_elemIndex_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1750: flag=_wrap_DynamicsRegressorParameter_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1751: flag=_wrap_DynamicsRegressorParameter_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1752: flag=_wrap_DynamicsRegressorParameter_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1753: flag=_wrap_DynamicsRegressorParameter_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1754: flag=_wrap_DynamicsRegressorParameter_ne(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1755: flag=_wrap_new_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1756: flag=_wrap_delete_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1757: flag=_wrap_DynamicsRegressorParametersList_parameters_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1758: flag=_wrap_DynamicsRegressorParametersList_parameters_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1759: flag=_wrap_DynamicsRegressorParametersList_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1760: flag=_wrap_DynamicsRegressorParametersList_addParam(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1761: flag=_wrap_DynamicsRegressorParametersList_addList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1762: flag=_wrap_DynamicsRegressorParametersList_findParam(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1763: flag=_wrap_DynamicsRegressorParametersList_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1764: flag=_wrap_new_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1765: flag=_wrap_delete_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1766: flag=_wrap_new_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1767: flag=_wrap_delete_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1768: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1769: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1770: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1771: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1772: flag=_wrap_DynamicsRegressorGenerator_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1773: flag=_wrap_DynamicsRegressorGenerator_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1774: flag=_wrap_DynamicsRegressorGenerator_getNrOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1775: flag=_wrap_DynamicsRegressorGenerator_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1776: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1777: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1778: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutput(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1779: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1780: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1781: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1782: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1783: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1784: flag=_wrap_DynamicsRegressorGenerator_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1785: flag=_wrap_DynamicsRegressorGenerator_getNrOfFakeLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1786: flag=_wrap_DynamicsRegressorGenerator_getBaseLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1787: flag=_wrap_DynamicsRegressorGenerator_getSensorsModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1788: flag=_wrap_DynamicsRegressorGenerator_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1789: flag=_wrap_DynamicsRegressorGenerator_getSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1790: flag=_wrap_DynamicsRegressorGenerator_setTorqueSensorMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1791: flag=_wrap_DynamicsRegressorGenerator_computeRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1792: flag=_wrap_DynamicsRegressorGenerator_getModelParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1793: flag=_wrap_DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1794: flag=_wrap_DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1795: flag=_wrap_DynamicsRegressorGenerator_generate_random_regressors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1796: flag=_wrap_new_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1797: flag=_wrap_delete_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1798: flag=_wrap_KinDynComputations_loadRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1799: flag=_wrap_KinDynComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1800: flag=_wrap_KinDynComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1801: flag=_wrap_KinDynComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1802: flag=_wrap_KinDynComputations_setFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1803: flag=_wrap_KinDynComputations_getFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1804: flag=_wrap_KinDynComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1805: flag=_wrap_KinDynComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1806: flag=_wrap_KinDynComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1807: flag=_wrap_KinDynComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1808: flag=_wrap_KinDynComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1809: flag=_wrap_KinDynComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1810: flag=_wrap_KinDynComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1811: flag=_wrap_KinDynComputations_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1812: flag=_wrap_KinDynComputations_getRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1813: flag=_wrap_KinDynComputations_getRelativeJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1814: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1815: flag=_wrap_KinDynComputations_setJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1816: flag=_wrap_KinDynComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1817: flag=_wrap_KinDynComputations_getRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1818: flag=_wrap_KinDynComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1819: flag=_wrap_KinDynComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1820: flag=_wrap_KinDynComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1821: flag=_wrap_KinDynComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1822: flag=_wrap_KinDynComputations_getModelVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1823: flag=_wrap_KinDynComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1824: flag=_wrap_KinDynComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1825: flag=_wrap_KinDynComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1826: flag=_wrap_KinDynComputations_getRelativeTransformExplicit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1827: flag=_wrap_KinDynComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1828: flag=_wrap_KinDynComputations_getFrameVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1829: flag=_wrap_KinDynComputations_getFrameAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1830: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1831: flag=_wrap_KinDynComputations_getRelativeJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1832: flag=_wrap_KinDynComputations_getRelativeJacobianExplicit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1833: flag=_wrap_KinDynComputations_getFrameBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1834: flag=_wrap_KinDynComputations_getCenterOfMassPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1835: flag=_wrap_KinDynComputations_getCenterOfMassVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1836: flag=_wrap_KinDynComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1837: flag=_wrap_KinDynComputations_getCenterOfMassBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1838: flag=_wrap_KinDynComputations_getAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1839: flag=_wrap_KinDynComputations_getAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1840: flag=_wrap_KinDynComputations_getCentroidalAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1841: flag=_wrap_KinDynComputations_getCentroidalAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1842: flag=_wrap_KinDynComputations_getLinearAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1843: flag=_wrap_KinDynComputations_getLinearAngularMomentumJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1844: flag=_wrap_KinDynComputations_getCentroidalTotalMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1845: flag=_wrap_KinDynComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1846: flag=_wrap_KinDynComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1847: flag=_wrap_KinDynComputations_generalizedBiasForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1848: flag=_wrap_KinDynComputations_generalizedGravityForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1849: flag=_wrap_KinDynComputations_generalizedExternalForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1850: flag=_wrap_KinDynComputations_inverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1851: flag=_wrap_delete_ICamera(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1852: flag=_wrap_ICamera_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1853: flag=_wrap_ICamera_setTarget(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1854: flag=_wrap_ICamera_setUpVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1855: flag=_wrap_ColorViz_r_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1856: flag=_wrap_ColorViz_r_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1857: flag=_wrap_ColorViz_g_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1858: flag=_wrap_ColorViz_g_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1859: flag=_wrap_ColorViz_b_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1860: flag=_wrap_ColorViz_b_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1861: flag=_wrap_ColorViz_a_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1862: flag=_wrap_ColorViz_a_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1863: flag=_wrap_new_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1864: flag=_wrap_delete_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1865: flag=_wrap_delete_ILight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1866: flag=_wrap_ILight_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1867: flag=_wrap_ILight_setType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1868: flag=_wrap_ILight_getType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1869: flag=_wrap_ILight_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1870: flag=_wrap_ILight_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1871: flag=_wrap_ILight_setDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1872: flag=_wrap_ILight_getDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1873: flag=_wrap_ILight_setAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1874: flag=_wrap_ILight_getAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1875: flag=_wrap_ILight_setSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1876: flag=_wrap_ILight_getSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1877: flag=_wrap_ILight_setDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1878: flag=_wrap_ILight_getDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1879: flag=_wrap_delete_IEnvironment(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1880: flag=_wrap_IEnvironment_getElements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1881: flag=_wrap_IEnvironment_setElementVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1882: flag=_wrap_IEnvironment_setBackgroundColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1883: flag=_wrap_IEnvironment_setAmbientLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1884: flag=_wrap_IEnvironment_getLights(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1885: flag=_wrap_IEnvironment_addLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1886: flag=_wrap_IEnvironment_lightViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1887: flag=_wrap_IEnvironment_removeLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1888: flag=_wrap_delete_IJetsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1889: flag=_wrap_IJetsVisualization_setJetsFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1890: flag=_wrap_IJetsVisualization_getNrOfJets(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1891: flag=_wrap_IJetsVisualization_getJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1892: flag=_wrap_IJetsVisualization_setJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1893: flag=_wrap_IJetsVisualization_setJetColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1894: flag=_wrap_IJetsVisualization_setJetsDimensions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1895: flag=_wrap_IJetsVisualization_setJetsIntensity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1896: flag=_wrap_delete_IVectorsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1897: flag=_wrap_IVectorsVisualization_addVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1898: flag=_wrap_IVectorsVisualization_getNrOfVectors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1899: flag=_wrap_IVectorsVisualization_getVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1900: flag=_wrap_IVectorsVisualization_updateVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1901: flag=_wrap_IVectorsVisualization_setVectorColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1902: flag=_wrap_IVectorsVisualization_setVectorsAspect(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1903: flag=_wrap_delete_IModelVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1904: flag=_wrap_IModelVisualization_setPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1905: flag=_wrap_IModelVisualization_setLinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1906: flag=_wrap_IModelVisualization_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1907: flag=_wrap_IModelVisualization_getInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1908: flag=_wrap_IModelVisualization_setModelVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1909: flag=_wrap_IModelVisualization_setModelColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1910: flag=_wrap_IModelVisualization_resetModelColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1911: flag=_wrap_IModelVisualization_setLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1912: flag=_wrap_IModelVisualization_resetLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1913: flag=_wrap_IModelVisualization_getLinkNames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1914: flag=_wrap_IModelVisualization_setLinkVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1915: flag=_wrap_IModelVisualization_getFeatures(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1916: flag=_wrap_IModelVisualization_setFeatureVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1917: flag=_wrap_IModelVisualization_jets(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1918: flag=_wrap_IModelVisualization_getWorldModelTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1919: flag=_wrap_IModelVisualization_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1920: flag=_wrap_VisualizerOptions_verbose_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1921: flag=_wrap_VisualizerOptions_verbose_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1922: flag=_wrap_VisualizerOptions_winWidth_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1923: flag=_wrap_VisualizerOptions_winWidth_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1924: flag=_wrap_VisualizerOptions_winHeight_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1925: flag=_wrap_VisualizerOptions_winHeight_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1926: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1927: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1928: flag=_wrap_new_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1929: flag=_wrap_delete_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1930: flag=_wrap_new_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1931: flag=_wrap_delete_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1932: flag=_wrap_Visualizer_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1933: flag=_wrap_Visualizer_getNrOfVisualizedModels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1934: flag=_wrap_Visualizer_getModelInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1935: flag=_wrap_Visualizer_getModelInstanceIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1936: flag=_wrap_Visualizer_addModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1937: flag=_wrap_Visualizer_modelViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1938: flag=_wrap_Visualizer_camera(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1939: flag=_wrap_Visualizer_enviroment(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1940: flag=_wrap_Visualizer_vectors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1941: flag=_wrap_Visualizer_run(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1942: flag=_wrap_Visualizer_draw(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1943: flag=_wrap_Visualizer_drawToFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1944: flag=_wrap_Visualizer_close(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1945: flag=_wrap_new_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1946: flag=_wrap_delete_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1947: flag=_wrap_DynamicsComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1948: flag=_wrap_DynamicsComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1949: flag=_wrap_DynamicsComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1950: flag=_wrap_DynamicsComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1951: flag=_wrap_DynamicsComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1952: flag=_wrap_DynamicsComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1953: flag=_wrap_DynamicsComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1954: flag=_wrap_DynamicsComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1955: flag=_wrap_DynamicsComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1956: flag=_wrap_DynamicsComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1957: flag=_wrap_DynamicsComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1958: flag=_wrap_DynamicsComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1959: flag=_wrap_DynamicsComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1960: flag=_wrap_DynamicsComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1961: flag=_wrap_DynamicsComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1962: flag=_wrap_DynamicsComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1963: flag=_wrap_DynamicsComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1964: flag=_wrap_DynamicsComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1965: flag=_wrap_DynamicsComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1966: flag=_wrap_DynamicsComputations_getFrameTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1967: flag=_wrap_DynamicsComputations_getFrameTwistInWorldOrient(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1968: flag=_wrap_DynamicsComputations_getFrameProperSpatialAcceleration(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1969: flag=_wrap_DynamicsComputations_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1970: flag=_wrap_DynamicsComputations_getLinkInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1971: flag=_wrap_DynamicsComputations_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1972: flag=_wrap_DynamicsComputations_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1973: flag=_wrap_DynamicsComputations_getJointLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1974: flag=_wrap_DynamicsComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1975: flag=_wrap_DynamicsComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1976: flag=_wrap_DynamicsComputations_getFrameJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1977: flag=_wrap_DynamicsComputations_getDynamicsRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1978: flag=_wrap_DynamicsComputations_getModelDynamicsParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1979: flag=_wrap_DynamicsComputations_getCenterOfMass(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1980: flag=_wrap_DynamicsComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
   default: flag=1, SWIG_Error(SWIG_RuntimeError, "No function id %d.", fcn_id);
   }
   if (flag) {
diff --git a/bindings/matlab/tests/EKFTest.m b/bindings/matlab/tests/EKFTest.m
new file mode 100644
index 00000000000..3501a5e35d4
--- /dev/null
+++ b/bindings/matlab/tests/EKFTest.m
@@ -0,0 +1,30 @@
+function test_suite=EKFTest
+    initTestSuite
+
+function test_span
+    y = iDynTree.Vector3;
+    x = 2.0;
+    y.setVal(1, x);
+    y_span = iDynTree.DynamicSpan(y.data, y.size);
+    x_from_span = y_span.getVal(1);
+    assertElementsAlmostEqual(x, x_from_span);
+
+    x1 = 20;
+    y_span.setVal(0, x1);
+    x1_from_span = y_span.getVal(0);
+    assertElementsAlmostEqual(x1, x1_from_span);
+
+function test_span_qekf
+    ekf = iDynTree.AttitudeQuaternionEKF();
+    ekf.initializeFilter();
+    init_orientation = iDynTree.Vector4;
+    init_orientation.setVal(0, 1.0);
+    init_span = iDynTree.DynamicSpan(init_orientation.data, init_orientation.size);
+    assertElementsAlmostEqual(init_span.getVal(0), 1.0);
+    ekf.setInternalStateInitialOrientation(init_span);
+
+    x = iDynTree.Vector10;
+    x.zero();
+    x_span = iDynTree.DynamicSpan(x.data, x.size);
+    ekf.ekfGetStates(x_span);
+    assertElementsAlmostEqual(x_span.getVal(0), 1.0);
diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 008a5097098..8c1fd314669 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -39,3 +39,10 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * All of the above addressed in the PR (https://github.com/robotology/idyntree/pull/516)
 * Added `getWorldFrameTransform` implementation in `SimpleLeggedOdometry` class
 * Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame
+* Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
+* Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
+
+### `core`
+* Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)
+* Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)
+* Added basic tests for `Span` (https://github.com/robotology/idyntree/pull/522)
diff --git a/src/core/include/iDynTree/Core/Span.h b/src/core/include/iDynTree/Core/Span.h
index 0c595809a57..0a68234eb49 100644
--- a/src/core/include/iDynTree/Core/Span.h
+++ b/src/core/include/iDynTree/Core/Span.h
@@ -67,11 +67,18 @@
 
 #endif                          // _MSC_VER
 
+// constexpr workaround for SWIG
+#ifdef SWIG
+#define IDYNTREE_CONSTEXPR
+#else
+#define IDYNTREE_CONSTEXPR constexpr
+#endif
+
 namespace iDynTree
 {
 
 // [views.constants], constants
-constexpr const std::ptrdiff_t dynamic_extent = -1;
+IDYNTREE_CONSTEXPR const std::ptrdiff_t dynamic_extent = -1;
 
 template <class ElementType, std::ptrdiff_t Extent = dynamic_extent>
 class Span;
@@ -138,120 +145,120 @@ namespace details
 
         span_iterator() = default;
 
-        constexpr span_iterator(const Span* span, typename Span::index_type idx) noexcept
+        IDYNTREE_CONSTEXPR span_iterator(const Span* span, typename Span::index_type idx) noexcept
             : span_(span), index_(idx)
         {}
 
         friend span_iterator<Span, true>;
         template<bool B, std::enable_if_t<!B && IsConst>* = nullptr>
-        constexpr span_iterator(const span_iterator<Span, B>& other) noexcept
+        IDYNTREE_CONSTEXPR span_iterator(const span_iterator<Span, B>& other) noexcept
             : span_iterator(other.span_, other.index_)
         {
         }
 
-        constexpr reference operator*() const
+        IDYNTREE_CONSTEXPR reference operator*() const
         {
             assert(index_ != span_->size());
             return *(span_->data() + index_);
         }
 
-        constexpr pointer operator->() const
+        IDYNTREE_CONSTEXPR pointer operator->() const
         {
             assert(index_ != span_->size());
             return span_->data() + index_;
         }
 
-        constexpr span_iterator& operator++()
+        IDYNTREE_CONSTEXPR span_iterator& operator++()
         {
             assert(0 <= index_ && index_ != span_->size());
             ++index_;
             return *this;
         }
 
-        constexpr span_iterator operator++(int)
+        IDYNTREE_CONSTEXPR span_iterator operator++(int)
         {
             auto ret = *this;
             ++(*this);
             return ret;
         }
 
-        constexpr span_iterator& operator--()
+        IDYNTREE_CONSTEXPR span_iterator& operator--()
         {
             assert(index_ != 0 && index_ <= span_->size());
             --index_;
             return *this;
         }
 
-        constexpr span_iterator operator--(int)
+        IDYNTREE_CONSTEXPR span_iterator operator--(int)
         {
             auto ret = *this;
             --(*this);
             return ret;
         }
 
-        constexpr span_iterator operator+(difference_type n) const
+        IDYNTREE_CONSTEXPR span_iterator operator+(difference_type n) const
         {
             auto ret = *this;
             return ret += n;
         }
 
-        constexpr span_iterator& operator+=(difference_type n)
+        IDYNTREE_CONSTEXPR span_iterator& operator+=(difference_type n)
         {
             assert((index_ + n) >= 0 && (index_ + n) <= span_->size());
             index_ += n;
             return *this;
         }
 
-        constexpr span_iterator operator-(difference_type n) const
+        IDYNTREE_CONSTEXPR span_iterator operator-(difference_type n) const
         {
             auto ret = *this;
             return ret -= n;
         }
 
-        constexpr span_iterator& operator-=(difference_type n) { return *this += -n; }
+        IDYNTREE_CONSTEXPR span_iterator& operator-=(difference_type n) { return *this += -n; }
 
-        constexpr difference_type operator-(span_iterator rhs) const
+        IDYNTREE_CONSTEXPR difference_type operator-(span_iterator rhs) const
         {
             assert(span_ == rhs.span_);
             return index_ - rhs.index_;
         }
 
-        constexpr reference operator[](difference_type n) const
+        IDYNTREE_CONSTEXPR reference operator[](difference_type n) const
         {
             return *(*this + n);
         }
 
-        constexpr friend bool operator==(span_iterator lhs,
+       IDYNTREE_CONSTEXPR friend bool operator==(span_iterator lhs,
                                          span_iterator rhs) noexcept
         {
             return lhs.span_ == rhs.span_ && lhs.index_ == rhs.index_;
         }
 
-        constexpr friend bool operator!=(span_iterator lhs,
+        IDYNTREE_CONSTEXPR friend bool operator!=(span_iterator lhs,
                                          span_iterator rhs) noexcept
         {
             return !(lhs == rhs);
         }
 
-        constexpr friend bool operator<(span_iterator lhs,
+        IDYNTREE_CONSTEXPR friend bool operator<(span_iterator lhs,
                                         span_iterator rhs) noexcept
         {
             return lhs.index_ < rhs.index_;
         }
 
-        constexpr friend bool operator<=(span_iterator lhs,
+        IDYNTREE_CONSTEXPR friend bool operator<=(span_iterator lhs,
                                          span_iterator rhs) noexcept
         {
             return !(rhs < lhs);
         }
 
-        constexpr friend bool operator>(span_iterator lhs,
+        IDYNTREE_CONSTEXPR friend bool operator>(span_iterator lhs,
                                         span_iterator rhs) noexcept
         {
             return rhs < lhs;
         }
 
-        constexpr friend bool operator>=(span_iterator lhs,
+        IDYNTREE_CONSTEXPR friend bool operator>=(span_iterator lhs,
                                          span_iterator rhs) noexcept
         {
             return !(rhs > lhs);
@@ -263,7 +270,7 @@ namespace details
     };
 
     template <class Span, bool IsConst>
-    constexpr span_iterator<Span, IsConst>
+    IDYNTREE_CONSTEXPR span_iterator<Span, IsConst>
     operator+(typename span_iterator<Span, IsConst>::difference_type n,
               span_iterator<Span, IsConst> rhs)
     {
@@ -271,7 +278,7 @@ namespace details
     }
 
     template <class Span, bool IsConst>
-    constexpr span_iterator<Span, IsConst>
+    IDYNTREE_CONSTEXPR span_iterator<Span, IsConst>
     operator-(typename span_iterator<Span, IsConst>::difference_type n,
               span_iterator<Span, IsConst> rhs)
     {
@@ -286,19 +293,19 @@ namespace details
 
         static_assert(Ext >= 0, "A fixed-size span must be >= 0 in size.");
 
-        constexpr extent_type() noexcept {}
+        IDYNTREE_CONSTEXPR extent_type() noexcept {}
 
         template <index_type Other>
-        constexpr extent_type(extent_type<Other> ext)
+        IDYNTREE_CONSTEXPR extent_type(extent_type<Other> ext)
         {
             static_assert(Other == Ext || Other == dynamic_extent,
                           "Mismatch between fixed-size extent and size of initializing data.");
             assert(ext.size() == Ext);
         }
 
-        constexpr extent_type(index_type size) { assert(size == Ext); }
+        IDYNTREE_CONSTEXPR extent_type(index_type size) { assert(size == Ext); }
 
-        constexpr index_type size() const noexcept { return Ext; }
+        IDYNTREE_CONSTEXPR index_type size() const noexcept { return Ext; }
     };
 
     template <>
@@ -308,13 +315,13 @@ namespace details
         using index_type = std::ptrdiff_t;
 
         template <index_type Other>
-        explicit constexpr extent_type(extent_type<Other> ext) : size_(ext.size())
+        explicit IDYNTREE_CONSTEXPR extent_type(extent_type<Other> ext) : size_(ext.size())
         {
         }
 
-        explicit constexpr extent_type(index_type size) : size_(size) { assert(size >= 0); }
+        explicit IDYNTREE_CONSTEXPR extent_type(index_type size) : size_(size) { assert(size >= 0); }
 
-        constexpr index_type size() const noexcept { return size_; }
+        IDYNTREE_CONSTEXPR index_type size() const noexcept { return size_; }
 
     private:
         index_type size_;
@@ -352,49 +359,52 @@ class Span
     static constexpr index_type extent { Extent };
 #endif
 
+#ifndef SWIG
     // [span.cons], span constructors, copy, assignment, and destructor
     template <bool Dependent = false,
               // "Dependent" is needed to make "std::enable_if_t<Dependent || Extent <= 0>" SFINAE,
               // since "std::enable_if_t<Extent <= 0>" is ill-formed when Extent is greater than 0.
               class = std::enable_if_t<(Dependent || Extent <= 0)>>
-    constexpr Span() noexcept : storage_(nullptr, details::extent_type<0>())
+    IDYNTREE_CONSTEXPR Span() noexcept : storage_(nullptr, details::extent_type<0>())
     {
     }
+#endif
 
-    constexpr Span(pointer ptr, index_type count) : storage_(ptr, count) {}
+    IDYNTREE_CONSTEXPR Span(pointer ptr, index_type count) : storage_(ptr, count) {}
 
-    constexpr Span(pointer firstElem, pointer lastElem)
+    IDYNTREE_CONSTEXPR Span(pointer firstElem, pointer lastElem)
         : storage_(firstElem, std::distance(firstElem, lastElem))
     {
     }
 
     template <std::size_t N>
-    constexpr Span(element_type (&arr)[N]) noexcept
+    IDYNTREE_CONSTEXPR Span(element_type (&arr)[N]) noexcept
         : storage_(KnownNotNull{&arr[0]}, details::extent_type<N>())
     {
     }
 
     template <std::size_t N, class ArrayElementType = std::remove_const_t<element_type>>
-    constexpr Span(std::array<ArrayElementType, N>& arr) noexcept
+    IDYNTREE_CONSTEXPR Span(std::array<ArrayElementType, N>& arr) noexcept
         : storage_(&arr[0], details::extent_type<N>())
     {
     }
 
     template <std::size_t N>
-    constexpr Span(const std::array<std::remove_const_t<element_type>, N>& arr) noexcept
+    IDYNTREE_CONSTEXPR Span(const std::array<std::remove_const_t<element_type>, N>& arr) noexcept
         : storage_(&arr[0], details::extent_type<N>())
     {
     }
 
     // NB: the SFINAE here uses .data() as a incomplete/imperfect proxy for the requirement
     // on Container to be a contiguous sequence container.
+#ifndef SWIG
     template <class Container,
               class = std::enable_if_t<
                   !details::is_span<Container>::value && !details::is_std_array<Container>::value &&
                   std::is_convertible<typename Container::pointer, pointer>::value &&
                   std::is_convertible<typename Container::pointer,
                                       decltype(std::declval<Container>().data())>::value>>
-    constexpr Span(Container& cont) : Span(cont.data(), static_cast<index_type>(cont.size()))
+    IDYNTREE_CONSTEXPR Span(Container& cont) : Span(cont.data(), static_cast<index_type>(cont.size()))
     {
     }
 
@@ -404,61 +414,66 @@ class Span
                   std::is_convertible<typename Container::pointer, pointer>::value &&
                   std::is_convertible<typename Container::pointer,
                                       decltype(std::declval<Container>().data())>::value>>
-    constexpr Span(const Container& cont) : Span(cont.data(), static_cast<index_type>(cont.size()))
+    IDYNTREE_CONSTEXPR Span(const Container& cont) : Span(cont.data(), static_cast<index_type>(cont.size()))
     {
     }
+#endif
 
-    constexpr Span(const Span& other) noexcept = default;
+    IDYNTREE_CONSTEXPR Span(const Span& other) noexcept = default;
 
+#ifndef SWIG
     template <
         class OtherElementType, std::ptrdiff_t OtherExtent,
         class = std::enable_if_t<
             details::is_allowed_extent_conversion<OtherExtent, Extent>::value &&
             details::is_allowed_element_type_conversion<OtherElementType, element_type>::value>>
-    constexpr Span(const Span<OtherElementType, OtherExtent>& other)
+    IDYNTREE_CONSTEXPR Span(const Span<OtherElementType, OtherExtent>& other)
         : storage_(other.data(), details::extent_type<OtherExtent>(other.size()))
     {
     }
+#endif
 
     ~Span() noexcept = default;
-    constexpr Span& operator=(const Span& other) noexcept = default;
+    IDYNTREE_CONSTEXPR Span& operator=(const Span& other) noexcept = default;
 
     // [span.sub], span subviews
     template <std::ptrdiff_t Count>
-    constexpr Span<element_type, Count> first() const
+    IDYNTREE_CONSTEXPR Span<element_type, Count> first() const
     {
         assert(Count >= 0 && Count <= size());
         return {data(), Count};
     }
 
     template <std::ptrdiff_t Count>
-    constexpr Span<element_type, Count> last() const
+    IDYNTREE_CONSTEXPR Span<element_type, Count> last() const
     {
         assert(Count >= 0 && size() - Count >= 0);
         return {data() + (size() - Count), Count};
     }
 
+#ifndef SWIG
     template <std::ptrdiff_t Offset, std::ptrdiff_t Count = dynamic_extent>
-    constexpr auto subspan() const -> typename details::calculate_subspan_type<ElementType, Extent, Offset, Count>::type
+    IDYNTREE_CONSTEXPR auto subspan() const -> typename details::calculate_subspan_type<ElementType, Extent, Offset, Count>::type
     {
         assert((Offset >= 0 && size() - Offset >= 0) &&
                 (Count == dynamic_extent || (Count >= 0 && Offset + Count <= size())));
 
         return {data() + Offset, Count == dynamic_extent ? size() - Offset : Count};
     }
+#endif
 
-    constexpr Span<element_type, dynamic_extent> first(index_type count) const
+    IDYNTREE_CONSTEXPR Span<element_type, dynamic_extent> first(index_type count) const
     {
         assert(count >= 0 && count <= size());
         return {data(), count};
     }
 
-    constexpr Span<element_type, dynamic_extent> last(index_type count) const
+    IDYNTREE_CONSTEXPR Span<element_type, dynamic_extent> last(index_type count) const
     {
         return make_subspan(size() - count, dynamic_extent, subspan_selector<Extent>{});
     }
 
-    constexpr Span<element_type, dynamic_extent> subspan(index_type offset,
+    IDYNTREE_CONSTEXPR Span<element_type, dynamic_extent> subspan(index_type offset,
                                                          index_type count = dynamic_extent) const
     {
         return make_subspan(offset, count, subspan_selector<Extent>{});
@@ -466,36 +481,44 @@ class Span
 
 
     // [span.obs], span observers
-    constexpr index_type size() const noexcept { return storage_.size(); }
-    constexpr index_type size_bytes() const noexcept
+    IDYNTREE_CONSTEXPR index_type size() const noexcept { return storage_.size(); }
+    IDYNTREE_CONSTEXPR index_type size_bytes() const noexcept
     {
         return size() * static_cast<index_type>(sizeof(element_type));
     }
-    constexpr bool empty() const noexcept { return size() == 0; }
+    IDYNTREE_CONSTEXPR bool empty() const noexcept { return size() == 0; }
 
     // [span.elem], span element access
-    constexpr reference operator[](index_type idx) const
+    IDYNTREE_CONSTEXPR reference operator[](index_type idx) const
     {
         assert(idx >= 0 && idx < storage_.size());
         return data()[idx];
     }
 
-    constexpr reference at(index_type idx) const { return this->operator[](idx); }
-    constexpr reference operator()(index_type idx) const { return this->operator[](idx); }
-    constexpr pointer data() const noexcept { return storage_.data(); }
+    IDYNTREE_CONSTEXPR double getVal(index_type idx) const { return this->operator[](idx);}
+    IDYNTREE_CONSTEXPR bool setVal(index_type idx, double val)
+    {
+        assert(idx >= 0 && idx < storage_.size());
+        data()[idx] = val;
+        return true;
+    }
+
+    IDYNTREE_CONSTEXPR reference at(index_type idx) const { return this->operator[](idx); }
+    IDYNTREE_CONSTEXPR reference operator()(index_type idx) const { return this->operator[](idx); }
+    IDYNTREE_CONSTEXPR pointer data() const noexcept { return storage_.data(); }
 
     // [span.iter], span iterator support
-    constexpr iterator begin() const noexcept { return {this, 0}; }
-    constexpr iterator end() const noexcept { return {this, size()}; }
+    IDYNTREE_CONSTEXPR iterator begin() const noexcept { return {this, 0}; }
+    IDYNTREE_CONSTEXPR iterator end() const noexcept { return {this, size()}; }
 
-    constexpr const_iterator cbegin() const noexcept { return {this, 0}; }
-    constexpr const_iterator cend() const noexcept { return {this, size()}; }
+    IDYNTREE_CONSTEXPR const_iterator cbegin() const noexcept { return {this, 0}; }
+    IDYNTREE_CONSTEXPR const_iterator cend() const noexcept { return {this, size()}; }
 
-    constexpr reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; }
-    constexpr reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; }
+    IDYNTREE_CONSTEXPR reverse_iterator rbegin() const noexcept { return reverse_iterator{end()}; }
+    IDYNTREE_CONSTEXPR reverse_iterator rend() const noexcept { return reverse_iterator{begin()}; }
 
-    constexpr const_reverse_iterator crbegin() const noexcept { return const_reverse_iterator{cend()}; }
-    constexpr const_reverse_iterator crend() const noexcept { return const_reverse_iterator{cbegin()}; }
+    IDYNTREE_CONSTEXPR const_reverse_iterator crbegin() const noexcept { return const_reverse_iterator{cend()}; }
+    IDYNTREE_CONSTEXPR const_reverse_iterator crend() const noexcept { return const_reverse_iterator{cbegin()}; }
 
 private:
 
@@ -515,20 +538,20 @@ class Span
         // KnownNotNull parameter is needed to remove unnecessary null check
         // in subspans and constructors from arrays
         template <class OtherExtentType>
-        constexpr storage_type(KnownNotNull data, OtherExtentType ext) : ExtentType(ext), data_(data.p)
+        IDYNTREE_CONSTEXPR storage_type(KnownNotNull data, OtherExtentType ext) : ExtentType(ext), data_(data.p)
         {
              assert(ExtentType::size() >= 0);
         }
 
 
         template <class OtherExtentType>
-        constexpr storage_type(pointer data, OtherExtentType ext) : ExtentType(ext), data_(data)
+        IDYNTREE_CONSTEXPR storage_type(pointer data, OtherExtentType ext) : ExtentType(ext), data_(data)
         {
              assert(ExtentType::size() >= 0);
              assert(data || ExtentType::size() == 0);
         }
 
-        constexpr pointer data() const noexcept { return data_; }
+        IDYNTREE_CONSTEXPR pointer data() const noexcept { return data_; }
 
     private:
         pointer data_;
@@ -538,7 +561,7 @@ class Span
 
     // The rest is needed to remove unnecessary null check
     // in subspans and constructors from arrays
-    constexpr Span(KnownNotNull ptr, index_type count) : storage_(ptr, count) {}
+    IDYNTREE_CONSTEXPR Span(KnownNotNull ptr, index_type count) : storage_(ptr, count) {}
 
     template <std::ptrdiff_t CallerExtent>
     class subspan_selector {};
@@ -569,48 +592,48 @@ class Span
 
 #if defined(IDYNTREE_USE_STATIC_CONSTEXPR_WORKAROUND)
 template <class ElementType, std::ptrdiff_t Extent>
-constexpr const typename Span<ElementType, Extent>::index_type Span<ElementType, Extent>::extent;
+IDYNTREE_CONSTEXPR const typename Span<ElementType, Extent>::index_type Span<ElementType, Extent>::extent;
 #endif
 
 
 // [span.comparison], span comparison operators
 template <class ElementType, std::ptrdiff_t FirstExtent, std::ptrdiff_t SecondExtent>
-constexpr bool operator==(Span<ElementType, FirstExtent> l,
+IDYNTREE_CONSTEXPR bool operator==(Span<ElementType, FirstExtent> l,
                           Span<ElementType, SecondExtent> r)
 {
     return std::equal(l.begin(), l.end(), r.begin(), r.end());
 }
 
 template <class ElementType, std::ptrdiff_t Extent>
-constexpr bool operator!=(Span<ElementType, Extent> l,
+IDYNTREE_CONSTEXPR bool operator!=(Span<ElementType, Extent> l,
                           Span<ElementType, Extent> r)
 {
     return !(l == r);
 }
 
 template <class ElementType, std::ptrdiff_t Extent>
-constexpr bool operator<(Span<ElementType, Extent> l,
+IDYNTREE_CONSTEXPR bool operator<(Span<ElementType, Extent> l,
                          Span<ElementType, Extent> r)
 {
     return std::lexicographical_compare(l.begin(), l.end(), r.begin(), r.end());
 }
 
 template <class ElementType, std::ptrdiff_t Extent>
-constexpr bool operator<=(Span<ElementType, Extent> l,
+IDYNTREE_CONSTEXPR bool operator<=(Span<ElementType, Extent> l,
                           Span<ElementType, Extent> r)
 {
     return !(l > r);
 }
 
 template <class ElementType, std::ptrdiff_t Extent>
-constexpr bool operator>(Span<ElementType, Extent> l,
+IDYNTREE_CONSTEXPR bool operator>(Span<ElementType, Extent> l,
                          Span<ElementType, Extent> r)
 {
     return r < l;
 }
 
 template <class ElementType, std::ptrdiff_t Extent>
-constexpr bool operator>=(Span<ElementType, Extent> l,
+IDYNTREE_CONSTEXPR bool operator>=(Span<ElementType, Extent> l,
                           Span<ElementType, Extent> r)
 {
     return !(l < r);
@@ -624,6 +647,7 @@ namespace details
     // we should use a narrow_cast<> to go to std::size_t, but older compilers may not see it as
     // constexpr
     // and so will fail compilation of the template
+#ifndef SWIG
     template <class ElementType, std::ptrdiff_t Extent>
     struct calculate_byte_size
         : std::integral_constant<std::ptrdiff_t,
@@ -637,6 +661,7 @@ namespace details
         : std::integral_constant<std::ptrdiff_t, dynamic_extent>
     {
     };
+#endif
 }
 
 
@@ -644,43 +669,43 @@ namespace details
 // make_span() - Utility functions for creating spans
 //
 template <class ElementType>
-constexpr Span<ElementType> make_span(ElementType* ptr, typename Span<ElementType>::index_type count)
+IDYNTREE_CONSTEXPR Span<ElementType> make_span(ElementType* ptr, typename Span<ElementType>::index_type count)
 {
     return Span<ElementType>(ptr, count);
 }
 
 template <class ElementType>
-constexpr Span<ElementType> make_span(ElementType* firstElem, ElementType* lastElem)
+IDYNTREE_CONSTEXPR Span<ElementType> make_span(ElementType* firstElem, ElementType* lastElem)
 {
     return Span<ElementType>(firstElem, lastElem);
 }
 
 template <class ElementType, std::size_t N>
-constexpr Span<ElementType, N> make_span(ElementType (&arr)[N]) noexcept
+IDYNTREE_CONSTEXPR Span<ElementType, N> make_span(ElementType (&arr)[N]) noexcept
 {
     return Span<ElementType, N>(arr);
 }
 
 template <class Container>
-constexpr Span<typename Container::value_type> make_span(Container& cont)
+IDYNTREE_CONSTEXPR Span<typename Container::value_type> make_span(Container& cont)
 {
     return Span<typename Container::value_type>(cont);
 }
 
 template <class Container>
-constexpr Span<const typename Container::value_type> make_span(const Container& cont)
+IDYNTREE_CONSTEXPR Span<const typename Container::value_type> make_span(const Container& cont)
 {
     return Span<const typename Container::value_type>(cont);
 }
 
 template <class Ptr>
-constexpr Span<typename Ptr::element_type> make_span(Ptr& cont, std::ptrdiff_t count)
+IDYNTREE_CONSTEXPR Span<typename Ptr::element_type> make_span(Ptr& cont, std::ptrdiff_t count)
 {
     return Span<typename Ptr::element_type>(cont, count);
 }
 
 template <class Ptr>
-constexpr Span<typename Ptr::element_type> make_span(Ptr& cont)
+IDYNTREE_CONSTEXPR Span<typename Ptr::element_type> make_span(Ptr& cont)
 {
     return Span<typename Ptr::element_type>(cont);
 }
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h
index bb21d795ef5..d3c18cdf22c 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimator.h
@@ -25,6 +25,21 @@ namespace iDynTree
     typedef iDynTree::Vector4 UnitQuaternion;
     typedef iDynTree::Vector3 RPY;
 
+    /** @struct state internal state of the estimator
+         * @var state::m_orientation
+         * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation
+         * @var state::m_orientation
+         * angular velocity estimate in \f$ \mathbb{R}^3 \f$
+         * @var state::m_orientation
+         * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$
+         */
+        struct AttitudeEstimatorState
+        {
+            iDynTree::UnitQuaternion m_orientation;
+            iDynTree::Vector3 m_angular_velocity;
+            iDynTree::Vector3 m_gyroscope_bias;
+        };
+
     /**
      * @class IAttitudeEstimator generic interface for attitude estimator classes
      *
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
index 1b4b684dfea..aea096314ef 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
@@ -17,6 +17,41 @@
 #include <cmath>
 #include <vector>
 
+
+/**
+ *
+ * @brief check a valid measurement
+ * @param[in] a vector3
+ * @return bool true/false
+ */
+bool checkValidMeasurement(const iDynTree::Vector3& in, const std::string& measurement_type, bool check_also_zero_vector);
+
+/**
+ *
+ * @brief get unit vector
+ * @param[in] a vector3
+ * @return bool false if input vector has zero norm
+ */
+bool getUnitVector(const iDynTree::Vector3& in, iDynTree::Vector3& out);
+
+/**
+ *
+ * @brief checks if vector has NaN values
+ * any element of vector is NaN implies a NaN vector
+ * @param[in] vec vector3
+ * @return bool true/false
+ */
+bool isVectorNaN(const iDynTree::Vector3& vec);
+
+/**
+ *
+ * @brief checks if vector is a zero vector
+ *  all elements of vector are zero implies a zero vector
+ * @param[in] vec vector3
+ * @return bool true/false
+ */
+bool isZeroVector(const iDynTree::Vector3& vec);
+
 /**
  *
  * @brief computes the cross vector of two 3D vectors
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
index ece8984a2ea..a683604d2d2 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeMahonyFilter.h
@@ -117,7 +117,7 @@ class AttitudeMahonyFilter : public IAttitudeEstimator
      */
     bool setParameters(const AttitudeMahonyFilterParameters& params)
     {
-        m_params = params;
+        m_params_mahony = params;
         return true;
     }
 
@@ -125,7 +125,7 @@ class AttitudeMahonyFilter : public IAttitudeEstimator
      * @brief Get filter parameters as a struct.
      * @param[out] params object of AttitudeMahonyFilterParameters passed as reference
      */
-    void getParameters(AttitudeMahonyFilterParameters& params) {params = m_params;}
+    void getParameters(AttitudeMahonyFilterParameters& params) {params = m_params_mahony;}
 
     bool updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas,
                                       const iDynTree::GyroscopeMeasurements& gyroMeas) override;
@@ -142,23 +142,10 @@ class AttitudeMahonyFilter : public IAttitudeEstimator
     bool setInternalState(const iDynTree::Span<double> & stateBuffer) override;
     bool setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) override;
 
+protected:
+    AttitudeMahonyFilterParameters m_params_mahony;              ///< struct holding the Mahony filter parameters
+    AttitudeEstimatorState m_state_mahony, m_initial_state_mahony;
 private:
-    AttitudeMahonyFilterParameters m_params;              ///< struct holding the Mahony filter parameters
-
-    /** @struct state internal state of the estimator
-     * @var state::m_orientation
-     * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation
-     * @var state::m_orientation
-     * angular velocity estimate in \f$ \mathbb{R}^3 \f$
-     * @var state::m_orientation
-     * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$
-     */
-    struct {
-        iDynTree::UnitQuaternion m_orientation;
-        iDynTree::Vector3 m_angular_velocity;
-        iDynTree::Vector3 m_gyroscope_bias;
-    } m_state, m_initial_state;
-
     iDynTree::Rotation m_orientationInSO3;                ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body
     iDynTree::RPY m_orientationInRPY;                     ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$
 
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
index 7ce215bce2b..15ef2a90a6c 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
@@ -120,17 +120,18 @@ namespace iDynTree
          * @brief Get filter parameters as a struct.
          * @param[out] params object of AttitudeQuaternionEKFParameters passed as reference
          */
-        void getParameters(AttitudeQuaternionEKFParameters& params) {params = m_params;}
+        void getParameters(AttitudeQuaternionEKFParameters& params) {params = m_params_qekf;}
 
         /**
           * @brief Set filter parameters with the struct members.
-          * This resets filter since it also calls useMagnetometerMeasurements().
+          * This resets filter since it also calls useMagnetometerMeasurements(flag)
+          * (if the use_magnetometer_measurements flag has been toggled).
           * @param[in] params object of AttitudeQuaternionEKFParameters passed as a const reference
           * @return true/false if successful/not
           */
         void setParameters(const AttitudeQuaternionEKFParameters& params)
         {
-            m_params = params;
+            m_params_qekf = params;
             useMagnetometerMeasurements(params.use_magnetometer_measurements);
         }
 
@@ -145,18 +146,20 @@ namespace iDynTree
          * @brief set discretization time step in seconds
          * @param[in] time_step_in_seconds time step
          */
-        void setTimeStepInSeconds(double time_step_in_seconds) {m_params.time_step_in_seconds = time_step_in_seconds; }
+        void setTimeStepInSeconds(double time_step_in_seconds) {m_params_qekf.time_step_in_seconds = time_step_in_seconds; }
 
         /**
          * @brief set bias correlation time factor
          * @param[in] bias_correlation_time_factor time factor for bias evolution
          */
-        void setBiasCorrelationTimeFactor(double bias_correlation_time_factor) { m_params.bias_correlation_time_factor = bias_correlation_time_factor; }
+        void setBiasCorrelationTimeFactor(double bias_correlation_time_factor) { m_params_qekf.bias_correlation_time_factor = bias_correlation_time_factor; }
 
         /**
          * @brief set flag to use magnetometer measurements
          * @param[in] use_magnetometer_measurements enable/disable magnetometer measurements
-         * @note calling this method will reset the filter, reinitialize the filter and set the previous state as filter's initial state
+         * @note calling this method with the flag same as current flag value will not change anything,
+         *  meanwhile a new flag setting will reset the filter, reinitialize the filter and
+         *  set the previous state as filter's initial state and previous state covariance as filter's intial state covariance
          */
         bool useMagnetometerMeasurements(bool use_magnetometer_measurements);
 
@@ -217,6 +220,11 @@ namespace iDynTree
         bool setInternalState(const iDynTree::Span<double> & stateBuffer) override;
         bool setInternalStateInitialOrientation(const iDynTree::Span<double>& orientationBuffer) override;
 
+    protected:
+        AttitudeEstimatorState m_state_qekf, m_initial_state_qekf;
+        AttitudeQuaternionEKFParameters m_params_qekf;   ///< struct holding the QEKF parameters
+
+
     private:
         /**
          * discrete system propagation \f$ f(X, u) = f(X, y_gyro) \f$
@@ -301,22 +309,6 @@ namespace iDynTree
          */
         bool callEkfUpdate();
 
-        AttitudeQuaternionEKFParameters m_params;   ///< struct holding the QEKF parameters
-
-        /** @struct state internal state of the estimator
-         * @var state::m_orientation
-         * orientation estimate in \f$ \mathbb{R}^4 \f$ quaternion representation
-         * @var state::m_orientation
-         * angular velocity estimate in \f$ \mathbb{R}^3 \f$
-         * @var state::m_orientation
-         * gyroscope bias estimate in \f$ \mathbb{R}^3 \f$
-         */
-        struct {
-            iDynTree::UnitQuaternion m_orientation;
-            iDynTree::Vector3 m_angular_velocity;
-            iDynTree::Vector3 m_gyroscope_bias;
-        } m_state, m_initial_state;
-
         iDynTree::Rotation m_orientationInSO3;                   ///< orientation estimate as rotation matrix \f$ {^A}R_B \f$ where \f$ A \f$ is inertial frame and \f$ B \f$ is the frame attached to the body
         iDynTree::RPY m_orientationInRPY;                        ///< orientation estimate as a 3D vector in RPY representation, where \f$ {^A}R_B = Rot_z(yaw)Rot_y(pitch)Rot_x(roll) \f$
 
diff --git a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
index a6ea5e166cb..ffd418ed820 100644
--- a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
+++ b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
@@ -60,7 +60,7 @@ namespace iDynTree
      */
     class DiscreteExtendedKalmanFilterHelper
     {
-    protected:
+    public:
         DiscreteExtendedKalmanFilterHelper();
 
         /**
diff --git a/src/estimation/src/AttitudeEstimatorUtils.cpp b/src/estimation/src/AttitudeEstimatorUtils.cpp
index 6aaa50b177f..612c914cf0b 100644
--- a/src/estimation/src/AttitudeEstimatorUtils.cpp
+++ b/src/estimation/src/AttitudeEstimatorUtils.cpp
@@ -10,6 +10,69 @@
 
 #include <iDynTree/Estimation/AttitudeEstimatorUtils.h>
 #include <vector>
+#include <cmath>
+
+bool checkValidMeasurement(const iDynTree::Vector3& in, const std::string& measurement_type, bool check_also_zero_vector)
+{
+    if (check_also_zero_vector)
+    {
+        if (isZeroVector(in))
+        {
+            iDynTree::reportError("AttitudeMahonyFilter", "checkValidMeasurement",
+                                  (measurement_type + " measurements are invalid. Expecting a non-zero vector.").c_str());
+            return false;
+        }
+    }
+
+    if (isVectorNaN(in))
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "checkValidMeasurement",
+                              (measurement_type + " measurements are invalid. Has NaN elements.").c_str());
+        return false;
+    }
+
+    return true;
+}
+
+
+bool getUnitVector(const iDynTree::Vector3& in, iDynTree::Vector3& out)
+{
+    using iDynTree::toEigen;
+
+    double norm{toEigen(in).norm()};
+    if (norm == 0)
+    {
+        return false;
+    }
+
+    out = in;
+    toEigen(out).normalize();
+    return true;
+}
+
+bool isVectorNaN(const iDynTree::Vector3& vec)
+{
+    for (size_t i = 0; i < vec.size(); i++)
+    {
+        if (std::isnan(vec(i)))
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+bool isZeroVector(const iDynTree::Vector3& vec)
+{
+    for (size_t i = 0; i < vec.size(); i++)
+    {
+        if (vec(i) != 0)
+        {
+            return false;
+        }
+    }
+    return true;
+}
 
 iDynTree::Vector3 crossVector(const iDynTree::Vector3& a, const iDynTree::Vector3& b)
 {
@@ -93,7 +156,6 @@ iDynTree::UnitQuaternion composeQuaternion(const iDynTree::UnitQuaternion &q1, c
     out(1) = imagOut(0);
     out(2) = imagOut(1);
     out(3) = imagOut(2);
-    toEigen(out).normalize();
 
     return out;
 }
@@ -115,7 +177,6 @@ iDynTree::UnitQuaternion composeQuaternion2(const iDynTree::UnitQuaternion &q1,
 
     iDynTree::UnitQuaternion out;
     toEigen(out) = toEigen(mapofYQuaternionToXYQuaternion(q1))*toEigen(q2); // to be read as out = mapofYQuaternionToXYQuaternion(q1)*q2
-    toEigen(out).normalize();
 
     return out;
 }
diff --git a/src/estimation/src/AttitudeMahonyFilter.cpp b/src/estimation/src/AttitudeMahonyFilter.cpp
index 05cd2fa6b89..507949e0162 100644
--- a/src/estimation/src/AttitudeMahonyFilter.cpp
+++ b/src/estimation/src/AttitudeMahonyFilter.cpp
@@ -31,7 +31,10 @@ iDynTree::Matrix3x3 getAngVelSkewSymmetricMatrixFromMeasurements(iDynTree::Vecto
     iDynTree::Vector3 vectorial_estimate = iDynTree::Vector3(va_hat.data(), 3);
 
     ///< compute vectorial direction from the measurement normalized
-    toEigen(meas).normalize();
+    if (toEigen(meas).norm() != 1)
+    {
+        toEigen(meas).normalize();
+    }
 
     iDynTree::Matrix3x3 Adyn = getMatrixFromVectorVectorMultiplication(meas, vectorial_estimate);
     iDynTree::Matrix3x3 Sdyn;
@@ -47,7 +50,17 @@ bool iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(const iDynTree
 {
     using iDynTree::toEigen;
 
-    iDynTree::Matrix3x3 S_acc = getAngVelSkewSymmetricMatrixFromMeasurements(linAccMeas, m_gravity_direction, 1-m_params.confidence_magnetometer_measurements, m_orientationInSO3);
+    if (!checkValidMeasurement(linAccMeas, "linear acceleration", true)) { return false; }
+    if (!checkValidMeasurement(gyroMeas, "gyroscope", false)) { return false; }
+
+    iDynTree::Vector3 linAccMeasUnitVector;
+    if (!getUnitVector(linAccMeas, linAccMeasUnitVector))
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "updateFilterWithMeasurements", "Cannot retrieve unit vector from linear acceleration measuremnts.");
+        return false;
+    }
+
+    iDynTree::Matrix3x3 S_acc = getAngVelSkewSymmetricMatrixFromMeasurements(linAccMeasUnitVector, m_gravity_direction, 1-m_params_mahony.confidence_magnetometer_measurements, m_orientationInSO3);
     toEigen(m_omega_mes) = -toEigen(mapso3ToR3(S_acc));
     m_Omega_y = gyroMeas;
 
@@ -58,14 +71,31 @@ bool iDynTree::AttitudeMahonyFilter::updateFilterWithMeasurements(const iDynTree
 {
     using iDynTree::toEigen;
 
-    if (m_params.use_magnetometer_measurements == false)
+    if (m_params_mahony.use_magnetometer_measurements == false)
     {
-        iDynTree::reportWarning("AttitudeMahonyFilter", "updateFilterWithMeasurements", "useMagnetoMeterMeasurements set to false, using only accelerometer measurements");
         return updateFilterWithMeasurements(linAccMeas, gyroMeas);
     }
 
-    iDynTree::Matrix3x3 S_acc = getAngVelSkewSymmetricMatrixFromMeasurements(linAccMeas, m_gravity_direction, 1-m_params.confidence_magnetometer_measurements, m_orientationInSO3);
-    iDynTree::Matrix3x3 S_mag = getAngVelSkewSymmetricMatrixFromMeasurements(magMeas, m_earth_magnetic_field_direction, m_params.confidence_magnetometer_measurements, m_orientationInSO3);
+    if (!checkValidMeasurement(linAccMeas, "linear acceleration", true)) { return false; }
+    if (!checkValidMeasurement(gyroMeas, "gyroscope", false)) { return false; }
+    if (!checkValidMeasurement(magMeas, "magnetometer", true)) { return false; }
+
+    iDynTree::Vector3 linAccMeasUnitVector;
+    if (!getUnitVector(linAccMeas, linAccMeasUnitVector))
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "updateFilterWithMeasurements", "Cannot retrieve unit vector from linear acceleration measuremnts.");
+        return false;
+    }
+
+    iDynTree::Vector3 magMeasUnitVector;
+    if (!getUnitVector(magMeas, magMeasUnitVector))
+    {
+        iDynTree::reportError("AttitudeMahonyFilter", "updateFilterWithMeasurements", "Cannot retrieve unit vector from magnetometer measuremnts.");
+        return false;
+    }
+
+    iDynTree::Matrix3x3 S_acc = getAngVelSkewSymmetricMatrixFromMeasurements(linAccMeasUnitVector, m_gravity_direction, 1-m_params_mahony.confidence_magnetometer_measurements, m_orientationInSO3);
+    iDynTree::Matrix3x3 S_mag = getAngVelSkewSymmetricMatrixFromMeasurements(magMeasUnitVector, m_earth_magnetic_field_direction, m_params_mahony.confidence_magnetometer_measurements, m_orientationInSO3);
     iDynTree::Matrix3x3 S_meas;
 
     toEigen(S_meas) = toEigen(S_acc) + toEigen(S_mag);
@@ -78,43 +108,51 @@ bool iDynTree::AttitudeMahonyFilter::propagateStates()
 {
     using iDynTree::toEigen;
     iDynTree::Vector3 gyro_update_dyn;
-    auto q(toEigen(m_state.m_orientation));
-    auto Omega(toEigen(m_state.m_angular_velocity));
-    auto b(toEigen(m_state.m_gyroscope_bias));
+    auto q(toEigen(m_state_mahony.m_orientation));
+    auto Omega(toEigen(m_state_mahony.m_angular_velocity));
+    auto b(toEigen(m_state_mahony.m_gyroscope_bias));
     auto gyroUpdate(toEigen(gyro_update_dyn));
     auto Omega_y(toEigen(m_Omega_y));
     auto omega_mes(toEigen(m_omega_mes));
 
     // compute the correction from the measurements
-    gyroUpdate = Omega_y - b + (omega_mes*m_params.kp);
+    gyroUpdate = Omega_y - b + (omega_mes*m_params_mahony.kp);
     iDynTree::UnitQuaternion correction = pureQuaternion(gyro_update_dyn);
-    auto dq(toEigen(composeQuaternion2(m_state.m_orientation, correction)));
+    auto dq(toEigen(composeQuaternion2(m_state_mahony.m_orientation, correction)));
 
     // system dynamics equations
-    q = q + (dq*(m_params.time_step_in_seconds*0.5));
-    q.normalize();
+    q = q + (dq*(m_params_mahony.time_step_in_seconds*0.5));
+    if (q.norm() == 0)
+    {
+        reportError("AttitudeMahonyFilter", "propagateStates", "invalid quaternion with zero norm");
+        return false;
+    }
+    if (q.norm() != 1)
+    {
+        q.normalize();
+    }
     Omega = Omega_y - b;
-    b = b - (omega_mes*m_params.ki)*(m_params.time_step_in_seconds);
+    b = b - (omega_mes*m_params_mahony.ki)*(m_params_mahony.time_step_in_seconds);
 
-    m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation);
-    m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation).asRPY();
+    m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state_mahony.m_orientation);
+    m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state_mahony.m_orientation).asRPY();
     return true;
 }
 
 iDynTree::AttitudeMahonyFilter::AttitudeMahonyFilter()
 {
-    m_params.time_step_in_seconds = 0.01;
-    m_params.kp = 1.0;
-    m_params.ki = 0.0;
-    m_params.use_magnetometer_measurements = false;
-    m_params.confidence_magnetometer_measurements = 0.0;
+    m_params_mahony.time_step_in_seconds = 0.01;
+    m_params_mahony.kp = 1.0;
+    m_params_mahony.ki = 0.0;
+    m_params_mahony.use_magnetometer_measurements = false;
+    m_params_mahony.confidence_magnetometer_measurements = 0.0;
 
-    m_state.m_orientation.zero();
-    m_state.m_orientation(0) = 1.0;
-    m_state.m_angular_velocity.zero();
-    m_state.m_gyroscope_bias.zero();
+    m_state_mahony.m_orientation.zero();
+    m_state_mahony.m_orientation(0) = 1.0;
+    m_state_mahony.m_angular_velocity.zero();
+    m_state_mahony.m_gyroscope_bias.zero();
 
-    m_initial_state = m_state;
+    m_initial_state_mahony = m_state_mahony;
 
     m_gravity_direction.zero();
     m_gravity_direction(2) = 1.0; // pointing downwards
@@ -122,8 +160,8 @@ iDynTree::AttitudeMahonyFilter::AttitudeMahonyFilter()
     m_earth_magnetic_field_direction.zero();
     m_earth_magnetic_field_direction(2) = 1.0;
 
-    m_orientationInSO3.fromQuaternion(m_state.m_orientation);
-    m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation).asRPY();
+    m_orientationInSO3.fromQuaternion(m_state_mahony.m_orientation);
+    m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state_mahony.m_orientation).asRPY();
 
     m_omega_mes.zero();
     m_Omega_y.zero();
@@ -132,36 +170,36 @@ iDynTree::AttitudeMahonyFilter::AttitudeMahonyFilter()
 
 void iDynTree::AttitudeMahonyFilter::setTimeStepInSeconds(double timestepInSeconds)
 {
-    m_params.time_step_in_seconds = timestepInSeconds;
+    m_params_mahony.time_step_in_seconds = timestepInSeconds;
 }
 
 void iDynTree::AttitudeMahonyFilter::setConfidenceForMagnetometerMeasurements(double confidence)
 {
-    if (m_params.use_magnetometer_measurements == false)
+    if (m_params_mahony.use_magnetometer_measurements == false)
     {
         iDynTree::reportWarning("AttitudeMahonyFilter", "setConfidenceForMagnetometerMeasurements", "not using magnetometer measurements, setting confidence to zero.");
-        m_params.confidence_magnetometer_measurements = 0.0;
+        m_params_mahony.confidence_magnetometer_measurements = 0.0;
     }
-    m_params.confidence_magnetometer_measurements = confidence;
+    m_params_mahony.confidence_magnetometer_measurements = confidence;
 }
 
 void iDynTree::AttitudeMahonyFilter::setGainki(double ki)
 {
-    m_params.ki = ki;
+    m_params_mahony.ki = ki;
 }
 
 void iDynTree::AttitudeMahonyFilter::setGainkp(double kp)
 {
-    m_params.kp = kp;
+    m_params_mahony.kp = kp;
 }
 
 void iDynTree::AttitudeMahonyFilter::useMagnetoMeterMeasurements(bool flag)
 {
-    m_params.use_magnetometer_measurements = flag;
+    m_params_mahony.use_magnetometer_measurements = flag;
     if (flag == false)
     {
         iDynTree::reportWarning("AttitudeMahonyFilter", "useMagnetoMeterMeasurements", "setting confidence on magnetometer measurements to zero.");
-        m_params.confidence_magnetometer_measurements = 0.0;
+        m_params_mahony.confidence_magnetometer_measurements = 0.0;
     }
 }
 
@@ -173,46 +211,46 @@ void iDynTree::AttitudeMahonyFilter::setGravityDirection(const iDynTree::Directi
 
 bool iDynTree::AttitudeMahonyFilter::getDefaultInternalInitialState(const iDynTree::Span< double >& stateBuffer) const
 {
-    stateBuffer(0) = m_initial_state.m_orientation(0);
-    stateBuffer(1) = m_initial_state.m_orientation(1);
-    stateBuffer(2) = m_initial_state.m_orientation(2);
-    stateBuffer(3) = m_initial_state.m_orientation(3);
-    stateBuffer(4) = m_initial_state.m_angular_velocity(0);
-    stateBuffer(5) = m_initial_state.m_angular_velocity(1);
-    stateBuffer(6) = m_initial_state.m_angular_velocity(2);
-    stateBuffer(7) = m_initial_state.m_gyroscope_bias(0);
-    stateBuffer(8) = m_initial_state.m_gyroscope_bias(1);
-    stateBuffer(9) = m_initial_state.m_gyroscope_bias(2);
+    stateBuffer(0) = m_initial_state_mahony.m_orientation(0);
+    stateBuffer(1) = m_initial_state_mahony.m_orientation(1);
+    stateBuffer(2) = m_initial_state_mahony.m_orientation(2);
+    stateBuffer(3) = m_initial_state_mahony.m_orientation(3);
+    stateBuffer(4) = m_initial_state_mahony.m_angular_velocity(0);
+    stateBuffer(5) = m_initial_state_mahony.m_angular_velocity(1);
+    stateBuffer(6) = m_initial_state_mahony.m_angular_velocity(2);
+    stateBuffer(7) = m_initial_state_mahony.m_gyroscope_bias(0);
+    stateBuffer(8) = m_initial_state_mahony.m_gyroscope_bias(1);
+    stateBuffer(9) = m_initial_state_mahony.m_gyroscope_bias(2);
     return true;
 }
 
 bool iDynTree::AttitudeMahonyFilter::getInternalState(const iDynTree::Span< double >& stateBuffer) const
 {
-    stateBuffer(0) = m_state.m_orientation(0);
-    stateBuffer(1) = m_state.m_orientation(1);
-    stateBuffer(2) = m_state.m_orientation(2);
-    stateBuffer(3) = m_state.m_orientation(3);
-    stateBuffer(4) = m_state.m_angular_velocity(0);
-    stateBuffer(5) = m_state.m_angular_velocity(1);
-    stateBuffer(6) = m_state.m_angular_velocity(2);
-    stateBuffer(7) = m_state.m_gyroscope_bias(0);
-    stateBuffer(8) = m_state.m_gyroscope_bias(1);
-    stateBuffer(9) = m_state.m_gyroscope_bias(2);
+    stateBuffer(0) = m_state_mahony.m_orientation(0);
+    stateBuffer(1) = m_state_mahony.m_orientation(1);
+    stateBuffer(2) = m_state_mahony.m_orientation(2);
+    stateBuffer(3) = m_state_mahony.m_orientation(3);
+    stateBuffer(4) = m_state_mahony.m_angular_velocity(0);
+    stateBuffer(5) = m_state_mahony.m_angular_velocity(1);
+    stateBuffer(6) = m_state_mahony.m_angular_velocity(2);
+    stateBuffer(7) = m_state_mahony.m_gyroscope_bias(0);
+    stateBuffer(8) = m_state_mahony.m_gyroscope_bias(1);
+    stateBuffer(9) = m_state_mahony.m_gyroscope_bias(2);
     return true;
 }
 
 size_t iDynTree::AttitudeMahonyFilter::getInternalStateSize() const
 {
     size_t size = 0;
-    size += m_state.m_orientation.size();
-    size += m_state.m_angular_velocity.size();
-    size += m_state.m_gyroscope_bias.size();
+    size += m_state_mahony.m_orientation.size();
+    size += m_state_mahony.m_angular_velocity.size();
+    size += m_state_mahony.m_gyroscope_bias.size();
     return size;
 }
 
 bool iDynTree::AttitudeMahonyFilter::getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q)
 {
-    q = m_state.m_orientation;
+    q = m_state_mahony.m_orientation;
     return true;
 }
 
@@ -235,30 +273,30 @@ bool iDynTree::AttitudeMahonyFilter::setInternalState(const iDynTree::Span< doub
         iDynTree::reportError("AttitudeMahonyFilter", "setInternalState", "state size mismatch, using default state");
         return false;
     }
-    m_state.m_orientation(0) = stateBuffer(0);
-    m_state.m_orientation(1) = stateBuffer(1);
-    m_state.m_orientation(2) = stateBuffer(2);
-    m_state.m_orientation(3) = stateBuffer(3);
-    m_state.m_angular_velocity(0) = stateBuffer(4);
-    m_state.m_angular_velocity(1) = stateBuffer(5);
-    m_state.m_angular_velocity(2) = stateBuffer(6);
-    m_state.m_gyroscope_bias(0) = stateBuffer(7);
-    m_state.m_gyroscope_bias(1) = stateBuffer(8);
-    m_state.m_gyroscope_bias(2) = stateBuffer(9);
+    m_state_mahony.m_orientation(0) = stateBuffer(0);
+    m_state_mahony.m_orientation(1) = stateBuffer(1);
+    m_state_mahony.m_orientation(2) = stateBuffer(2);
+    m_state_mahony.m_orientation(3) = stateBuffer(3);
+    m_state_mahony.m_angular_velocity(0) = stateBuffer(4);
+    m_state_mahony.m_angular_velocity(1) = stateBuffer(5);
+    m_state_mahony.m_angular_velocity(2) = stateBuffer(6);
+    m_state_mahony.m_gyroscope_bias(0) = stateBuffer(7);
+    m_state_mahony.m_gyroscope_bias(1) = stateBuffer(8);
+    m_state_mahony.m_gyroscope_bias(2) = stateBuffer(9);
     return true;
 }
 
 bool iDynTree::AttitudeMahonyFilter::setInternalStateInitialOrientation(const iDynTree::Span< double >& orientationBuffer)
 {
-    if ((size_t)orientationBuffer.size() != m_state.m_orientation.size())
+    if ((size_t)orientationBuffer.size() != m_state_mahony.m_orientation.size())
     {
         iDynTree::reportError("AttitudeMahonyFilter", "setInternalStateInitialOrientation", "orientation size mismatch, using default state");
         return false;
     }
-    m_state.m_orientation(0) = orientationBuffer(0);
-    m_state.m_orientation(1) = orientationBuffer(1);
-    m_state.m_orientation(2) = orientationBuffer(2);
-    m_state.m_orientation(3) = orientationBuffer(3);
+    m_state_mahony.m_orientation(0) = orientationBuffer(0);
+    m_state_mahony.m_orientation(1) = orientationBuffer(1);
+    m_state_mahony.m_orientation(2) = orientationBuffer(2);
+    m_state_mahony.m_orientation(3) = orientationBuffer(3);
 
     return true;
 }
diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp
index 35209a01d87..a70c97af6cd 100644
--- a/src/estimation/src/AttitudeQuaternionEKF.cpp
+++ b/src/estimation/src/AttitudeQuaternionEKF.cpp
@@ -15,9 +15,9 @@ void iDynTree::AttitudeQuaternionEKF::serializeStateVector()
 {
     using iDynTree::toEigen;
     auto x(toEigen(m_x));
-    auto q(toEigen(m_state.m_orientation));
-    auto Omega(toEigen(m_state.m_angular_velocity));
-    auto b(toEigen(m_state.m_gyroscope_bias));
+    auto q(toEigen(m_state_qekf.m_orientation));
+    auto Omega(toEigen(m_state_qekf.m_angular_velocity));
+    auto b(toEigen(m_state_qekf.m_gyroscope_bias));
 
     if (m_x.size() != m_state_size)
     {
@@ -33,13 +33,13 @@ void iDynTree::AttitudeQuaternionEKF::deserializeStateVector()
 {
     using iDynTree::toEigen;
     auto x(toEigen(m_x));
-    auto q(toEigen(m_state.m_orientation));
-    auto Omega(toEigen(m_state.m_angular_velocity));
-    auto b(toEigen(m_state.m_gyroscope_bias));
+    auto q(toEigen(m_state_qekf.m_orientation));
+    auto Omega(toEigen(m_state_qekf.m_angular_velocity));
+    auto b(toEigen(m_state_qekf.m_gyroscope_bias));
 
     q = x.block<4, 1>(0, 0);
     Omega = x.block<3, 1>(4, 0);
-    b = x.block<3, 1>(4, 0);
+    b = x.block<3, 1>(7, 0);
 }
 
 void iDynTree::AttitudeQuaternionEKF::prepareSystemNoiseCovarianceMatrix(iDynTree::MatrixDynSize &Q)
@@ -52,7 +52,7 @@ void iDynTree::AttitudeQuaternionEKF::prepareSystemNoiseCovarianceMatrix(iDynTre
     }
 
 
-    iDynTree::MatrixDynSize Fu_dyn(m_state_size, m_input_size + m_state.m_gyroscope_bias.size());
+    iDynTree::MatrixDynSize Fu_dyn(m_state_size, m_input_size + m_state_qekf.m_gyroscope_bias.size());
     Fu_dyn.zero();
     iDynTree::Matrix6x6 U_dyn;
     U_dyn.zero();
@@ -65,8 +65,8 @@ void iDynTree::AttitudeQuaternionEKF::prepareSystemNoiseCovarianceMatrix(iDynTre
     F_u.block<3, 3>(4, 0) = Id3;
     F_u.block<3,3>(7, 3) = Id3;
 
-    U.block<3, 3>(0, 0) = Id3*m_params.gyroscope_noise_variance;
-    U.block<3, 3>(3, 3) = Id3*m_params.gyro_bias_noise_variance;
+    U.block<3, 3>(0, 0) = Id3*m_params_qekf.gyroscope_noise_variance;
+    U.block<3, 3>(3, 3) = Id3*m_params_qekf.gyro_bias_noise_variance;
 
     Q_ = F_u*U*F_u.transpose();
 }
@@ -84,26 +84,27 @@ void iDynTree::AttitudeQuaternionEKF::prepareMeasurementNoiseCovarianceMatrix(iD
     auto R_(toEigen(R));
     auto Id3(toEigen(m_Id3));
 
-    R_.block<3, 3>(0, 0) = Id3*m_params.accelerometer_noise_variance;
-    if (m_params.use_magnetometer_measurements)
+    R_.block<3, 3>(0, 0) = Id3*m_params_qekf.accelerometer_noise_variance;
+    if (m_params_qekf.use_magnetometer_measurements)
     {
-        R_(3, 3) = m_params.magnetometer_noise_variance;
+        R_(3, 3) = m_params_qekf.magnetometer_noise_variance;
     }
 }
 
 iDynTree::AttitudeQuaternionEKF::AttitudeQuaternionEKF()
 {
-    m_state.m_orientation.zero();
-    m_state.m_orientation(0) = 1.0;
-    m_state.m_angular_velocity.zero();
-    m_state.m_gyroscope_bias.zero();
+    m_state_qekf.m_orientation.zero();
+    m_state_qekf.m_orientation(0) = 1.0;
+    m_state_qekf.m_angular_velocity.zero();
+    m_state_qekf.m_gyroscope_bias.zero();
 
-    m_initial_state = m_state;
-    m_orientationInSO3.fromQuaternion(m_state.m_orientation);
+    m_initial_state_qekf = m_state_qekf;
+    m_orientationInSO3.fromQuaternion(m_state_qekf.m_orientation);
     m_orientationInRPY = m_orientationInSO3.asRPY();
 
     m_Omega_y.zero();
     m_Acc_y.zero();
+    m_Acc_y(2) = 1; // TODO: validate this assumption at initial step
 
     m_gravity_direction.zero();
     m_gravity_direction(2) = 1;
@@ -117,7 +118,7 @@ bool iDynTree::AttitudeQuaternionEKF::initializeFilter()
 {
     m_state_size = this->getInternalStateSize();
 
-    if (m_params.use_magnetometer_measurements)
+    if (m_params_qekf.use_magnetometer_measurements)
     {
         m_output_size = output_dimensions_with_magnetometer;
     }
@@ -143,21 +144,21 @@ bool iDynTree::AttitudeQuaternionEKF::initializeFilter()
     }
 
     // once the buffers are resized and zeroed, setup the matrices
-    if (!setInitialStateCovariance(m_params.initial_orientation_error_variance,
-                                   m_params.initial_ang_vel_error_variance,
-                                   m_params.initial_gyro_bias_error_variance))
+    if (!setInitialStateCovariance(m_params_qekf.initial_orientation_error_variance,
+                                   m_params_qekf.initial_ang_vel_error_variance,
+                                   m_params_qekf.initial_gyro_bias_error_variance))
     {
         return false;
     }
 
-    if (!setSystemNoiseVariance(m_params.gyroscope_noise_variance,
-                            m_params.gyro_bias_noise_variance))
+    if (!setSystemNoiseVariance(m_params_qekf.gyroscope_noise_variance,
+                            m_params_qekf.gyro_bias_noise_variance))
     {
         return false;
     }
 
-    if (!setMeasurementNoiseVariance(m_params.accelerometer_noise_variance,
-                                m_params.magnetometer_noise_variance))
+    if (!setMeasurementNoiseVariance(m_params_qekf.accelerometer_noise_variance,
+                                m_params_qekf.magnetometer_noise_variance))
     {
         return false;
     }
@@ -175,7 +176,7 @@ bool iDynTree::AttitudeQuaternionEKF::propagateStates()
     if (ekfGetStates(x_span))
     {
         deserializeStateVector();
-        m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation);
+        m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state_qekf.m_orientation);
         m_orientationInRPY = m_orientationInSO3.asRPY();
     }
     else
@@ -195,7 +196,7 @@ void iDynTree::AttitudeQuaternionEKF::serializeMeasurementVector()
     }
 
     toEigen(m_y).block<3, 1>(0, 0) = toEigen(m_Acc_y);
-    if (m_params.use_magnetometer_measurements)
+    if (m_params_qekf.use_magnetometer_measurements)
     {
         m_y(3) = m_Mag_y;
     }
@@ -212,7 +213,7 @@ bool iDynTree::AttitudeQuaternionEKF::callEkfUpdate()
     if (ekfGetStates(x_span))
     {
         deserializeStateVector();
-        m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state.m_orientation);
+        m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state_qekf.m_orientation);
         m_orientationInRPY = m_orientationInSO3.asRPY();
     }
     else
@@ -230,8 +231,19 @@ bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTre
     {
         m_y.resize(m_output_size);
     }
-    m_Acc_y = linAccMeas;
-    toEigen(m_Acc_y).normalize();
+
+    if (!checkValidMeasurement(linAccMeas, "linear acceleration", true)) { return false; }
+    if (!checkValidMeasurement(gyroMeas, "gyroscope", false)) { return false; }
+    if (!checkValidMeasurement(magMeas, "magnetometer", true)) { return false; }
+
+    iDynTree::Vector3 linAccMeasUnitVector;
+    if (!getUnitVector(linAccMeas, linAccMeasUnitVector))
+    {
+        iDynTree::reportError("AttitudeQuaternionEKF", "updateFilterWithMeasurements", "Cannot retrieve unit vector from linear acceleration measuremnts.");
+        return false;
+    }
+
+    m_Acc_y = linAccMeasUnitVector;
     m_Omega_y = gyroMeas;
 
     // compute yaw angle from magnetometer measurements by limiting the vertical influence of magentometer
@@ -239,11 +251,17 @@ bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTre
     // convert back to body frame and compute the yaw using atan2(y, x)
     iDynTree::Vector3 mag_meas_in_inertial_frame;
     iDynTree::Vector3 modified_mag_meas_in_body_frame;
+    iDynTree::Vector3 magMeasUnitVector;
+    if (!getUnitVector(magMeas, magMeasUnitVector))
+    {
+        iDynTree::reportError("AttitudeQuaternionEKF", "updateFilterWithMeasurements", "Cannot retrieve unit vector from magnetometer measuremnts.");
+        return false;
+    }
 
     using iDynTree::toEigen;
     auto m_A(toEigen(mag_meas_in_inertial_frame));
     auto m_B_modified(toEigen(modified_mag_meas_in_body_frame));
-    auto m_yB(toEigen(magMeas));
+    auto m_yB(toEigen(magMeasUnitVector));
     auto A_R_B(toEigen(m_orientationInSO3));
 
     m_A = A_R_B * m_yB;
@@ -262,8 +280,9 @@ bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTre
 
 bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTree::LinearAccelerometerMeasurements& linAccMeas, const iDynTree::GyroscopeMeasurements& gyroMeas)
 {
-    iDynTree::MagnetometerMeasurements magMeas;
+    iDynTree::MagnetometerMeasurements magMeas; // dummy measurement
     magMeas.zero();
+    magMeas(2) = 1;
     bool ok = updateFilterWithMeasurements(linAccMeas, gyroMeas, magMeas);
     return ok;
 }
@@ -293,14 +312,14 @@ bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSiz
     toEigen(gyro_bias) = toEigen(x).block<3,1>(7, 0);
 
     iDynTree::Matrix4x4 dfq_by_dq;
-    dfq_by_dq(0,0) = dfq_by_dq(1, 1) = dfq_by_dq(2, 2) = dfq_by_dq(3, 3) = (2.0/m_params.time_step_in_seconds);
+    dfq_by_dq(0,0) = dfq_by_dq(1, 1) = dfq_by_dq(2, 2) = dfq_by_dq(3, 3) = (2.0/m_params_qekf.time_step_in_seconds);
     dfq_by_dq(1, 0) = dfq_by_dq(2, 3) = ang_vel(0);
     dfq_by_dq(2, 0) = dfq_by_dq(3, 1) = ang_vel(1);
     dfq_by_dq(3, 0) = dfq_by_dq(1, 2) = ang_vel(2);
     dfq_by_dq(0, 1) = dfq_by_dq(3, 2) = -ang_vel(0);
     dfq_by_dq(0, 2) = dfq_by_dq(1, 3) = -ang_vel(1);
     dfq_by_dq(0, 3) = dfq_by_dq(2, 1) = -ang_vel(2);
-    toEigen(dfq_by_dq) *= (m_params.time_step_in_seconds/2.0);
+    toEigen(dfq_by_dq) *= (m_params_qekf.time_step_in_seconds/2.0);
 
     iDynTree::MatrixDynSize dfq_by_dangvel;
     dfq_by_dangvel.resize(4, 3);
@@ -311,13 +330,13 @@ bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSiz
     dfq_by_dangvel(1, 2) = q(2);
     dfq_by_dangvel(3, 1) = q(1);
     dfq_by_dangvel(1, 0) = dfq_by_dangvel(2, 1) = dfq_by_dangvel(3, 2) = -q(0);
-    toEigen(dfq_by_dangvel) *= (m_params.time_step_in_seconds/2.0);
+    toEigen(dfq_by_dangvel) *= (m_params_qekf.time_step_in_seconds/2.0);
 
     iDynTree::Matrix3x3 dfangvel_by_dgyrobias(m_Id3);
     toEigen(dfangvel_by_dgyrobias) *= -1;
 
     iDynTree::Matrix3x3 dfgyrobias_by_dgyrobias(m_Id3);
-    toEigen(dfgyrobias_by_dgyrobias) *= (1 - (m_params.bias_correlation_time_factor*m_params.time_step_in_seconds));
+    toEigen(dfgyrobias_by_dgyrobias) *= (1 - (m_params_qekf.bias_correlation_time_factor*m_params_qekf.time_step_in_seconds));
 
     toEigen(F).block<4,4>(0,0) = toEigen(dfq_by_dq);
     toEigen(F).block<4,3>(0,4) = toEigen(dfq_by_dangvel);
@@ -401,13 +420,13 @@ bool iDynTree::AttitudeQuaternionEKF::ekf_f(const iDynTree::VectorDynSize& x_k,
 {
     if (x_k.size() != m_x.size() || xhat_k_plus_one.size() != m_x.size())
     {
-        reportError("AttitudeQuaternionEKF", "f", "state size mismatch");
+        reportError("AttitudeQuaternionEKF", "ekf_f", "state size mismatch");
         return false;
     }
 
     if (u_k.size() != m_u.size())
     {
-        reportError("AttitudeQuaternionEKF", "f", "input size mismatch");
+        reportError("AttitudeQuaternionEKF", "ekf_f", "input size mismatch");
         return false;
     }
 
@@ -429,9 +448,18 @@ bool iDynTree::AttitudeQuaternionEKF::ekf_f(const iDynTree::VectorDynSize& x_k,
     iDynTree::UnitQuaternion correction = pureQuaternion(ang_vel);
     auto dq(toEigen(composeQuaternion2(orientation, correction)));
 
-    x_hat_plus.block<4,1>(0, 0) = q + (dq*(m_params.time_step_in_seconds*0.5));
+    x_hat_plus.block<4,1>(0, 0) = q + (dq*(m_params_qekf.time_step_in_seconds*0.5));
+    if (x_hat_plus.block<4,1>(0, 0).norm() == 0)
+    {
+        reportError("AttitudeQuaternionEKF", "ekf_f", "invalid quaternion");
+        return false;
+    }
+    if (q.norm() != 1)
+    {
+        q.normalize();
+    }
     x_hat_plus.block<3,1>(4, 0) = u - b;
-    x_hat_plus.block<3,1>(7, 0) = b*(1 - (m_params.bias_correlation_time_factor*m_params.time_step_in_seconds));
+    x_hat_plus.block<3,1>(7, 0) = b*(1 - (m_params_qekf.bias_correlation_time_factor*m_params_qekf.time_step_in_seconds));
 
     return true;
 }
@@ -492,8 +520,8 @@ bool iDynTree::AttitudeQuaternionEKF::ekf_h(const iDynTree::VectorDynSize& xhat_
 
 bool iDynTree::AttitudeQuaternionEKF::setMeasurementNoiseVariance(double acc, double mag)
 {
-    m_params.accelerometer_noise_variance = acc;
-    m_params.magnetometer_noise_variance = mag;
+    m_params_qekf.accelerometer_noise_variance = acc;
+    m_params_qekf.magnetometer_noise_variance = mag;
 
     iDynTree::MatrixDynSize R = iDynTree::MatrixDynSize(m_output_size, m_output_size);
     prepareMeasurementNoiseCovarianceMatrix(R);
@@ -505,8 +533,8 @@ bool iDynTree::AttitudeQuaternionEKF::setMeasurementNoiseVariance(double acc, do
 
 bool iDynTree::AttitudeQuaternionEKF::setSystemNoiseVariance(double gyro, double gyro_bias)
 {
-    m_params.gyroscope_noise_variance = gyro;
-    m_params.gyro_bias_noise_variance = gyro_bias;
+    m_params_qekf.gyroscope_noise_variance = gyro;
+    m_params_qekf.gyro_bias_noise_variance = gyro_bias;
 
     iDynTree::MatrixDynSize Q = iDynTree::MatrixDynSize(m_state_size, m_state_size);
     prepareSystemNoiseCovarianceMatrix(Q);
@@ -537,47 +565,47 @@ bool iDynTree::AttitudeQuaternionEKF::setInitialStateCovariance(double orientati
 
 bool iDynTree::AttitudeQuaternionEKF::getDefaultInternalInitialState(const iDynTree::Span< double >& stateBuffer) const
 {
-    stateBuffer(0) = m_initial_state.m_orientation(0);
-    stateBuffer(1) = m_initial_state.m_orientation(1);
-    stateBuffer(2) = m_initial_state.m_orientation(2);
-    stateBuffer(3) = m_initial_state.m_orientation(3);
-    stateBuffer(4) = m_initial_state.m_angular_velocity(0);
-    stateBuffer(5) = m_initial_state.m_angular_velocity(1);
-    stateBuffer(6) = m_initial_state.m_angular_velocity(2);
-    stateBuffer(7) = m_initial_state.m_gyroscope_bias(0);
-    stateBuffer(8) = m_initial_state.m_gyroscope_bias(1);
-    stateBuffer(9) = m_initial_state.m_gyroscope_bias(2);
+    stateBuffer(0) = m_initial_state_qekf.m_orientation(0);
+    stateBuffer(1) = m_initial_state_qekf.m_orientation(1);
+    stateBuffer(2) = m_initial_state_qekf.m_orientation(2);
+    stateBuffer(3) = m_initial_state_qekf.m_orientation(3);
+    stateBuffer(4) = m_initial_state_qekf.m_angular_velocity(0);
+    stateBuffer(5) = m_initial_state_qekf.m_angular_velocity(1);
+    stateBuffer(6) = m_initial_state_qekf.m_angular_velocity(2);
+    stateBuffer(7) = m_initial_state_qekf.m_gyroscope_bias(0);
+    stateBuffer(8) = m_initial_state_qekf.m_gyroscope_bias(1);
+    stateBuffer(9) = m_initial_state_qekf.m_gyroscope_bias(2);
     return true;
 }
 
 bool iDynTree::AttitudeQuaternionEKF::getInternalState(const iDynTree::Span< double >& stateBuffer) const
 {
-    stateBuffer(0) = m_state.m_orientation(0);
-    stateBuffer(1) = m_state.m_orientation(1);
-    stateBuffer(2) = m_state.m_orientation(2);
-    stateBuffer(3) = m_state.m_orientation(3);
-    stateBuffer(4) = m_state.m_angular_velocity(0);
-    stateBuffer(5) = m_state.m_angular_velocity(1);
-    stateBuffer(6) = m_state.m_angular_velocity(2);
-    stateBuffer(7) = m_state.m_gyroscope_bias(0);
-    stateBuffer(8) = m_state.m_gyroscope_bias(1);
-    stateBuffer(9) = m_state.m_gyroscope_bias(2);
+    stateBuffer(0) = m_state_qekf.m_orientation(0);
+    stateBuffer(1) = m_state_qekf.m_orientation(1);
+    stateBuffer(2) = m_state_qekf.m_orientation(2);
+    stateBuffer(3) = m_state_qekf.m_orientation(3);
+    stateBuffer(4) = m_state_qekf.m_angular_velocity(0);
+    stateBuffer(5) = m_state_qekf.m_angular_velocity(1);
+    stateBuffer(6) = m_state_qekf.m_angular_velocity(2);
+    stateBuffer(7) = m_state_qekf.m_gyroscope_bias(0);
+    stateBuffer(8) = m_state_qekf.m_gyroscope_bias(1);
+    stateBuffer(9) = m_state_qekf.m_gyroscope_bias(2);
     return true;
 }
 
 std::size_t iDynTree::AttitudeQuaternionEKF::getInternalStateSize() const
 {
     size_t size = 0;
-    size += m_state.m_orientation.size();
-    size += m_state.m_angular_velocity.size();
-    size += m_state.m_gyroscope_bias.size();
+    size += m_state_qekf.m_orientation.size();
+    size += m_state_qekf.m_angular_velocity.size();
+    size += m_state_qekf.m_gyroscope_bias.size();
 
     return size;
 }
 
 bool iDynTree::AttitudeQuaternionEKF::getOrientationEstimateAsQuaternion(iDynTree::UnitQuaternion& q)
 {
-    q = m_state.m_orientation;
+    q = m_state_qekf.m_orientation;
     return true;
 }
 
@@ -600,18 +628,18 @@ bool iDynTree::AttitudeQuaternionEKF::setInternalState(const iDynTree::Span< dou
         iDynTree::reportError("AttitudeQuaternionEKF", "setInternalState", "state size mismatch, using default state");
         return false;
     }
-    m_state.m_orientation(0) = stateBuffer(0);
-    m_state.m_orientation(1) = stateBuffer(1);
-    m_state.m_orientation(2) = stateBuffer(2);
-    m_state.m_orientation(3) = stateBuffer(3);
-    m_state.m_angular_velocity(0) = stateBuffer(4);
-    m_state.m_angular_velocity(1) = stateBuffer(5);
-    m_state.m_angular_velocity(2) = stateBuffer(6);
-    m_state.m_gyroscope_bias(0) = stateBuffer(7);
-    m_state.m_gyroscope_bias(1) = stateBuffer(8);
-    m_state.m_gyroscope_bias(2) = stateBuffer(9);
-
-    m_initial_state = m_state;
+    m_state_qekf.m_orientation(0) = stateBuffer(0);
+    m_state_qekf.m_orientation(1) = stateBuffer(1);
+    m_state_qekf.m_orientation(2) = stateBuffer(2);
+    m_state_qekf.m_orientation(3) = stateBuffer(3);
+    m_state_qekf.m_angular_velocity(0) = stateBuffer(4);
+    m_state_qekf.m_angular_velocity(1) = stateBuffer(5);
+    m_state_qekf.m_angular_velocity(2) = stateBuffer(6);
+    m_state_qekf.m_gyroscope_bias(0) = stateBuffer(7);
+    m_state_qekf.m_gyroscope_bias(1) = stateBuffer(8);
+    m_state_qekf.m_gyroscope_bias(2) = stateBuffer(9);
+
+    m_initial_state_qekf = m_state_qekf;
     serializeStateVector();
     iDynTree::Span<double> x0_span(m_x.data(), m_x.size());
     bool ok = ekfSetInitialState(x0_span);
@@ -620,28 +648,54 @@ bool iDynTree::AttitudeQuaternionEKF::setInternalState(const iDynTree::Span< dou
 
 bool iDynTree::AttitudeQuaternionEKF::setInternalStateInitialOrientation(const iDynTree::Span< double >& orientationBuffer)
 {
-    if ((size_t)orientationBuffer.size() != m_state.m_orientation.size())
+    if ((size_t)orientationBuffer.size() != m_state_qekf.m_orientation.size())
     {
         iDynTree::reportError("AttitudeQuaternionEKF", "setInternalStateInitialOrientation", "orientation size mismatch, using default state");
         return false;
     }
-    m_state.m_orientation(0) = orientationBuffer(0);
-    m_state.m_orientation(1) = orientationBuffer(1);
-    m_state.m_orientation(2) = orientationBuffer(2);
-    m_state.m_orientation(3) = orientationBuffer(3);
 
-    return true;
+    m_state_qekf.m_orientation(0) = orientationBuffer(0);
+    m_state_qekf.m_orientation(1) = orientationBuffer(1);
+    m_state_qekf.m_orientation(2) = orientationBuffer(2);
+    m_state_qekf.m_orientation(3) = orientationBuffer(3);
+
+    m_initial_state_qekf = m_state_qekf;
+    serializeStateVector();
+    iDynTree::Span<double> x0_span(m_x.data(), m_x.size());
+    bool ok = ekfSetInitialState(x0_span);
+
+    return ok;
 }
 
 
 bool iDynTree::AttitudeQuaternionEKF::useMagnetometerMeasurements(bool use_magnetometer_measurements)
 {
-    m_params.use_magnetometer_measurements = use_magnetometer_measurements;
+    if (use_magnetometer_measurements == m_params_qekf.use_magnetometer_measurements)
+    {
+        return true;
+    }
+
+    // store current state estimate and variance
+    iDynTree::VectorDynSize x(m_x);
+    iDynTree::MatrixDynSize P(m_x.size(), m_x.size());
+    iDynTree::Span<double> P_span(P.data(), P.capacity());
+    if (!ekfGetStateCovariance(P_span))
+    {
+        return false;
+    }
+
+    // get variances
+    double orientation_var{P(0,0)};
+    double ang_vel_var{P(4,4)};
+    double gyro_bias_var{P(7,7)};
+
+    m_params_qekf.use_magnetometer_measurements = use_magnetometer_measurements;
     ekfReset();
 
     bool ok = initializeFilter();
-    iDynTree::Span<double> x_span(m_x.data(), m_x.size());
+    iDynTree::Span<double> x_span(x.data(), x.size());
     ok = setInternalState(x_span) && ok;
+    ok = setInitialStateCovariance(orientation_var, ang_vel_var, gyro_bias_var) && ok;
     return ok;
 }
 
diff --git a/src/estimation/src/ExtendedKalmanFilter.cpp b/src/estimation/src/ExtendedKalmanFilter.cpp
index dd6fc6ddb7f..854994343d2 100644
--- a/src/estimation/src/ExtendedKalmanFilter.cpp
+++ b/src/estimation/src/ExtendedKalmanFilter.cpp
@@ -252,6 +252,7 @@ bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStates(const iDynTree::
 
     for (size_t i = 0; i < m_dim_X; i++)
     {
+        double temp{m_x(i)};
         x(i) = m_x(i);
     }
     return true;
@@ -265,20 +266,9 @@ bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStateCovariance(const i
         return false;
     }
 
-    // TODO: replace this really bad way of copying a matrix to a span
-    std::vector<double> temp;
-    temp.resize(P.size());
-    for (size_t i = 0; i < m_P.rows(); i++)
+    for (size_t i = 0; i < m_P.capacity(); i++)
     {
-        for (size_t j = 0; j < m_P.cols(); j++)
-        {
-            temp.push_back(m_P(i, j));
-        }
-    }
-
-    for (size_t k = 0; k < temp.size(); k++)
-    {
-        P(k) = temp[k];
+        P(i) = m_P.data()[i];
     }
 
     return true;
diff --git a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
index 2f6f10ad9b7..e4473bde9bb 100644
--- a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
+++ b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
@@ -22,6 +22,9 @@ void run(iDynTree::IAttitudeEstimator* estimator,
     estimator->propagateStates();
     std::cout << "Update measurements..." << std::endl;
     estimator->updateFilterWithMeasurements(acc, gyro, mag);
+    iDynTree::RPY rpy;
+    estimator->getOrientationEstimateAsRPY(rpy);
+    std::cout << "Estimated orientation in RPY: " << rpy.toString() << std::endl;
 }
 
 int main()
@@ -31,6 +34,17 @@ int main()
     qEKF = std::make_unique<iDynTree::AttitudeQuaternionEKF>();
     iDynTree::AttitudeQuaternionEKFParameters params;
 
+    params.time_step_in_seconds = 0.010;
+    params.accelerometer_noise_variance = 0.03;
+    params.magnetometer_noise_variance = 0.0;
+    params.gyroscope_noise_variance = 0.5;
+    params.gyro_bias_noise_variance = 10e-11;
+    params.initial_orientation_error_variance = 10e-6;
+    params.initial_ang_vel_error_variance = 10e-1;
+    params.initial_gyro_bias_error_variance = 10e-11;
+    params.bias_correlation_time_factor = 10e-3;
+    params.use_magnetometer_measurements = false;
+
     size_t x_size = qEKF->getInternalStateSize();
 
     // calling setParams resets and intializes the filter
@@ -38,18 +52,18 @@ int main()
     bool ok = qEKF->initializeFilter();
     ASSERT_IS_TRUE(ok);
     std::cout << "Propagate states will internally run EKF predict step" << std::endl;
-    std::cout << "if setParams() was not called before, calling propagateStates before setting internal state will throw initial state not set error...." << std::endl;
+    std::cout << "calling propagateStates before setting internal state will throw initial state not set error...." << std::endl;
     ok = qEKF->propagateStates();
-    ASSERT_IS_TRUE(ok);
+    ASSERT_IS_FALSE(ok);
     std::cout << "Print.... OK" << std::endl;
 
     iDynTree::VectorDynSize x0;
     x0.resize(10);
     x0.zero();
-    x0(0) = 0.5;
-    x0(1) = -0.5;
-    x0(2) = 0.5;
-    x0(3) = -0.5;
+    x0(0) = 1.0;
+    x0(1) = 0.0;
+    x0(2) = 0.0;
+    x0(3) = 0.0;
 
     iDynTree::Span<double> x0_span(x0.data(), x0.size());
     std::cout << "Setting initial internal state" << std::endl;
@@ -65,6 +79,10 @@ int main()
     iDynTree::LinAcceleration linAcc; linAcc.zero();
     iDynTree::GyroscopeMeasurements gyro; gyro.zero();
     iDynTree::MagnetometerMeasurements mag; mag.zero();
+    ok = qEKF->updateFilterWithMeasurements(linAcc, gyro, mag);
+    ASSERT_IS_FALSE(ok);
+    linAcc(2) = 1.0;
+    mag(2) = 1.0;
     std::cout << "Update measurements will internally run EKF update step" << std::endl;
     ok = qEKF->updateFilterWithMeasurements(linAcc, gyro, mag);
     ASSERT_IS_TRUE(ok);
@@ -74,11 +92,29 @@ int main()
     ok = qEKF->updateFilterWithMeasurements(linAcc, gyro);
     ASSERT_IS_TRUE(ok);
 
+    x0(0) = 1.0;
+    x0(1) = 0.0;
+    x0(2) = 0.0;
+    x0(3) = 0.0;
+    iDynTree::Span<double> x1_span(x0.data(), x0.size());
+    std::cout << "Setting initial internal state" << std::endl;
+    ok = qEKF->setInternalState(x1_span);
+
     iDynTree::IAttitudeEstimator* qekf_(qEKF.get());
     for (int i = 0; i <10 ; i++ )
     {
         run(qekf_, linAcc, gyro, mag);
     }
 
+    linAcc.zero();
+    linAcc(1) = 1;
+    gyro(1) = 0.05;
+
+    for (int i = 0; i <10 ; i++ )
+    {
+        run(qekf_, linAcc, gyro, mag);
+    }
+
+
     return EXIT_SUCCESS;
 }

From 74c6f82ed135965d3396c9c652852f41ad3f9d7e Mon Sep 17 00:00:00 2001
From: Gabriele Nava <gabriele.nava@mail.polimi.it>
Date: Wed, 17 Apr 2019 10:18:38 +0200
Subject: [PATCH 102/194] Add matlab/octave high level wrappers (#530)

---
 README.md                                     |   4 +
 bindings/matlab/+iDynTreeWrappers/README.md   |  77 ++++++
 .../+iDynTreeWrappers/generalizedBiasForces.m |  39 +++
 .../generalizedGravityForces.m                |  39 +++
 .../matlab/+iDynTreeWrappers/getBaseTwist.m   |  27 ++
 .../getCenterOfMassJacobian.m                 |  35 +++
 .../getCenterOfMassPosition.m                 |  27 ++
 .../getCenterOfMassVelocity.m                 |  27 ++
 .../getCentroidalTotalMomentum.m              |  29 +++
 .../+iDynTreeWrappers/getFloatingBase.m       |  25 ++
 .../+iDynTreeWrappers/getFrameBiasAcc.m       |  29 +++
 .../getFrameFreeFloatingJacobian.m            |  38 +++
 .../matlab/+iDynTreeWrappers/getFrameIndex.m  |  25 ++
 .../matlab/+iDynTreeWrappers/getFrameName.m   |  25 ++
 .../getFrameVelocityRepresentation.m          |  52 ++++
 .../getFreeFloatingMassMatrix.m               |  59 +++++
 .../matlab/+iDynTreeWrappers/getJointPos.m    |  35 +++
 .../matlab/+iDynTreeWrappers/getJointVel.m    |  35 +++
 .../matlab/+iDynTreeWrappers/getModelVel.m    |  36 +++
 .../getNrOfDegreesOfFreedom.m                 |  38 +++
 .../+iDynTreeWrappers/getRelativeJacobian.m   |  41 +++
 .../+iDynTreeWrappers/getRelativeTransform.m  |  93 +++++++
 .../matlab/+iDynTreeWrappers/getRobotState.m  |  81 ++++++
 .../+iDynTreeWrappers/getWorldBaseTransform.m |  67 +++++
 .../+iDynTreeWrappers/getWorldTransform.m     |  68 +++++
 .../+iDynTreeWrappers/initializeVisualizer.m  |  42 ++++
 .../+iDynTreeWrappers/loadReducedModel.m      |  63 +++++
 .../+iDynTreeWrappers/setFloatingBase.m       |  29 +++
 .../setFrameVelocityRepresentation.m          |  55 +++++
 .../matlab/+iDynTreeWrappers/setJointPos.m    |  52 ++++
 .../matlab/+iDynTreeWrappers/setRobotState.m  | 233 ++++++++++++++++++
 .../+iDynTreeWrappers/updateVisualizer.m      | 127 ++++++++++
 .../+iDynTreeWrappers/visualizerSetup.m       |  53 ++++
 bindings/matlab/CMakeLists.txt                |  12 +-
 bindings/matlab/tests/CMakeLists.txt          |   6 +-
 .../matlab/tests/highLevelWrappersSmokeTest.m | 155 ++++++++++++
 doc/releases/v0_12.md                         |   3 +
 37 files changed, 1877 insertions(+), 4 deletions(-)
 create mode 100644 bindings/matlab/+iDynTreeWrappers/README.md
 create mode 100644 bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getBaseTwist.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFloatingBase.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameIndex.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameName.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getJointPos.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getJointVel.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getModelVel.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getRobotState.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/getWorldTransform.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/loadReducedModel.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/setFloatingBase.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/setJointPos.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/setRobotState.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/updateVisualizer.m
 create mode 100644 bindings/matlab/+iDynTreeWrappers/visualizerSetup.m
 create mode 100644 bindings/matlab/tests/highLevelWrappersSmokeTest.m

diff --git a/README.md b/README.md
index a2edcb8b449..58d4526ef6e 100644
--- a/README.md
+++ b/README.md
@@ -167,6 +167,10 @@ for example because you modified some iDynTree classes, you can install the expe
 version of Swig with Matlab support from https://github.com/robotology-dependencies/swig/ (branch `matlab`) and then enable Matlab bindings generation with the `IDYNTREE_GENERATE_MATLAB` options.
 For more info on how to modify the matlab bindings, see https://github.com/robotology/idyntree/blob/master/doc/dev/faqs.md#how-to-add-wrap-a-new-class-or-function-with-swig .
 
+##### Matlab/Octave high level wrappers
+They are a collection of Matlab/Octave functions that wraps the functionalities of (mainly) the iDyntree class `KinDynComputations` into functions with a typical Matlab/Octave interface. The purpose of the high-level wrappers is to provide a simpler and easy-to-use interface for Matlab/Octave users who want to use iDyntree inside Matlab/Octave, also helping in designing code which is less error-prone and easier to debug (e.g. in case the interface of an iDyntree function will change in the future). More details and a complete list of the wrappers can be found in the [wrappers README](/bindings/matlab/+iDynTreeWrappers/README.md).
+
+**Usage**: the wrappers package is installed together with the iDyntree bindings when compiling iDyntree with option `IDYNTREE_USES_MATLAB` or `IDYNTREE_USES_OCTAVE` set to `ON`. The functions can be called from Matlab/Octave using the namespace `iDynTreeWrappers`, i.e. `iDynTreeWrappers.name_of_the_corresponding_iDynTree_method`.
 
 ## Tutorials
 | Topic  | C++ | Matlab | Python |
diff --git a/bindings/matlab/+iDynTreeWrappers/README.md b/bindings/matlab/+iDynTreeWrappers/README.md
new file mode 100644
index 00000000000..ea4aff67a16
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/README.md
@@ -0,0 +1,77 @@
+# idyntree high-level-wrappers
+
+A collection of Matlab/Octave functions that wraps the functionalities of (mainly) the iDyntree class `KinDynComputations`. For further information on the single wrapper, refer to the description included in each function.
+
+## Motivations
+
+For a Matlab/Octave user, it may be sometimes counterintuitive to use C++ based formalism inside Matlab/Octave. Furthermore, there are common iDyntree functions that require several lines of code in order to be used correctly. E.g. see the `getRobotState` function:
+
+```
+    basePose_iDyntree   = iDynTree.Transform();
+    jointPos_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    baseVel_iDyntree    = iDynTree.Twist();
+    jointVel_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    gravityVec_iDyntree = iDynTree.Vector3();
+
+    KinDynModel.kinDynComp.getRobotState(basePose_iDyntree,jointPos_iDyntree,baseVel_iDyntree,jointVel_iDyntree,gravityVec_iDyntree);
+
+    baseRotation_iDyntree = basePose_iDyntree.getRotation;
+    baseOrigin_iDyntree   = basePose_iDyntree.getPosition;
+
+    baseRotation = baseRotation_iDyntree.toMatlab;
+    baseOrigin   = baseOrigin_iDyntree.toMatlab;
+    jointPos     = jointPos_iDyntree.toMatlab;
+    baseVel      = baseVel_iDyntree.toMatlab;
+    jointVel     = jointVel_iDyntree.toMatlab; 
+    basePose     = [baseRotation, baseOrigin;
+                       0,  0,  0,  1]; 
+```
+
+The purpose of the wrapper is therefore to provide a simpler and easy-to-use interface for Matlab/Octave users who wants to use iDyntree inside Matlab/Octave, also helping in designing code which is less error-prone and easier to debug (e.g. in case the interface of an iDyntree function will change in the future).
+
+## KinDynComputations class
+
+### Load the reduced model
+
+- [loadReducedModel](loadReducedModel.m)
+
+### Retrieve data from the model
+
+- [getJointPos](getJointPos.m)
+- [getJointVel](getJointVel.m)
+- [getCentroidalTotalMomentum](getCentroidalTotalMomentum.m)
+- [getRobotState](getRobotState.m)
+- [getWorldBaseTransform](getWorldBaseTransform.m)
+- [getBaseTwist](getBaseTwist.m) 
+- [getModelVel](getModelVel.m) 
+- [getFrameVelocityRepresentation](getFrameVelocityRepresentation.m)
+- [getNrOfDegreesOfFreedom](getNrOfDegreesOfFreedom.m)
+- [getFloatingBase](getFloatingBase.m)
+- [getFrameIndex](getFrameIndex.m)
+- [getFrameName](getFrameName.m)
+- [getWorldTransform](getWorldTransform.m) 
+- [getRelativeTransform](getRelativeTransform.m) 
+- [getRelativeJacobian](getRelativeJacobian.m) 
+- [getFreeFloatingMassMatrix](getFreeFloatingMassMatrix.m)
+- [getFrameBiasAcc](getFrameBiasAcc.m)
+- [getFrameFreeFloatingJacobian](getFrameFreeFloatingJacobian.m)
+- [getCenterOfMassPosition](getCenterOfMassPosition.m) 
+- [generalizedBiasForces](generalizedBiasForces.m) 
+- [generalizedGravityForces](generalizedGravityForces.m)
+- [getCenterOfMassJacobian](getCenterOfMassJacobian.m) 
+- [getCenterOfMassVelocity](getCenterOfMassVelocity.m) 
+ 
+### Set the model-related quantities
+ 
+- [setJointPos](setJointPos.m) 
+- [setFrameVelocityRepresentation](setFrameVelocityRepresentation.m)
+- [setFloatingBase](setFloatingBase.m) 
+- [setRobotState](setRobotState.m)
+
+## Visualizer class
+
+Not proper wrappers, they wrap more than one method of the class each. **Requirements:** compile iDyntree with Irrlicht `(IDYNTREE_USES_IRRLICHT = ON)`.
+
+- [initializeVisualizer](initializeVisualizer.m)
+- [visualizerSetup](visualizerSetup.m)
+- [updateVisualizer](updateVisualizer.m)
diff --git a/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m b/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m
new file mode 100644
index 00000000000..f19d619c305
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/generalizedBiasForces.m
@@ -0,0 +1,39 @@
+function h = generalizedBiasForces(KinDynModel)
+
+    % GENERALIZEDBIASFORCES retrieves the generalized bias forces from 
+    %                            the reduced model. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  h = generalizedBiasForces(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - h: [6+ndof x 1] generalized bias accelerations.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the vector that must be populated with the bias forces
+    h_iDyntree = iDynTree.FreeFloatingGeneralizedTorques(KinDynModel.kinDynComp.model);
+    
+    % get the bias forces
+    ack = KinDynModel.kinDynComp.generalizedBiasForces(h_iDyntree);
+    
+    % check for errors
+    if ~ack
+        error('[generalizedBiasForces]: unable to get the bias forces from the reduced model.')
+    end
+    
+    % convert to Matlab format: compute the base bias acc (h_b) and the
+    % joint bias acc (h_s) and concatenate them
+    h_b = h_iDyntree.baseWrench.toMatlab;
+    h_s = h_iDyntree.jointTorques.toMatlab;   
+    h   = [h_b;h_s];
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m b/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m
new file mode 100644
index 00000000000..eb0fadc58ef
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/generalizedGravityForces.m
@@ -0,0 +1,39 @@
+function g = generalizedGravityForces(KinDynModel)
+
+    % GENERALIZEDGRAVITYFORCES retrieves the generalized gravity forces 
+    %                               given the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  g = generalizedGravityForces(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - g: [6+ndof x 1] generalized gravity forces.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the vector that must be populated with the gravity forces
+    g_iDyntree = iDynTree.FreeFloatingGeneralizedTorques(KinDynModel.kinDynComp.model);
+    
+    % get the gravity forces
+    ack = KinDynModel.kinDynComp.generalizedGravityForces(g_iDyntree);
+    
+    % check for errors
+    if ~ack  
+        error('[generalizedGravityForces]: unable to get the gravity forces from the reduced model.')
+    end
+    
+    % convert to Matlab format: compute the base gravity forces (g_b) and the
+    % joint gravity forces (g_j) and concatenate them
+    g_b = g_iDyntree.baseWrench.toMatlab;
+    g_s = g_iDyntree.jointTorques.toMatlab;   
+    g   = [g_b; g_s];
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m b/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m
new file mode 100644
index 00000000000..cd6e4a66d38
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getBaseTwist.m
@@ -0,0 +1,27 @@
+function baseVel = getBaseTwist(KinDynModel)
+
+    % GETBASETWIST retrieves the robot base velocity from the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  baseVel = getBaseTwist(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - baseVel: [6 x 1] vector of base linear and angular velocity;
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % get the base velocities
+    baseVel_iDyntree = KinDynModel.kinDynComp.getBaseTwist();
+    
+    % convert to Matlab format
+    baseVel = baseVel_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m
new file mode 100644
index 00000000000..16a1c5df24d
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassJacobian.m
@@ -0,0 +1,35 @@
+function J_CoM = getCenterOfMassJacobian(KinDynModel)
+
+    % GETCENTEROFMASSJACOBIAN retrieves the CoM jacobian.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  J_CoM = getCenterOfMassJacobian(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - J_CoM: [3 x 6+ndof] CoM free floating Jacobian.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the matrix that must be populated with the jacobian map
+    J_CoM_iDyntree = iDynTree.MatrixDynSize(3,KinDynModel.NDOF+6);
+    
+    % get the free floating jacobian
+    ack = KinDynModel.kinDynComp.getCenterOfMassJacobian(J_CoM_iDyntree);
+    
+    % check for errors
+    if ~ack    
+        error('[getCenterOfMassJacobian]: unable to get the CoM Jacobian from the reduced model.')
+    end
+    
+    % convert to Matlab format
+    J_CoM = J_CoM_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m
new file mode 100644
index 00000000000..2268fedfe8e
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassPosition.m
@@ -0,0 +1,27 @@
+function posCoM = getCenterOfMassPosition(KinDynModel)
+
+    % GETCENTEROFMASSPOSITION retrieves the CoM position in world coordinates.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  posCoM = getCenterOfMassPosition(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - posCoM: [3 x 1] CoM position w.r.t. world frame.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the CoM position
+    posCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassPosition(); 
+    
+    % covert to matlab
+    posCoM = posCoM_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m
new file mode 100644
index 00000000000..f7fe44a3956
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getCenterOfMassVelocity.m
@@ -0,0 +1,27 @@
+function velCoM = getCenterOfMassVelocity(KinDynModel)
+
+    % GETCENTEROFMASSVELOCITY retrieves the CoM velocity in world coordinates.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  velCoM = getCenterOfMassVelocity(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - velCoM: [3 x 1] CoM velocity w.r.t. world frame.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------  
+    
+    % get the CoM velocity
+    velCoM_iDyntree = KinDynModel.kinDynComp.getCenterOfMassVelocity(); 
+    
+    % covert to matlab
+    velCoM = velCoM_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m b/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m
new file mode 100644
index 00000000000..f0a0e73783a
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getCentroidalTotalMomentum.m
@@ -0,0 +1,29 @@
+function totalMomentum = getCentroidalTotalMomentum(KinDynModel)
+
+    % GETCENTROIDALTOTALMOMENTUM retrieves the centroidal momentum from the reduced
+    %                                 model. The quantity is expressed in a frame that depends
+    %                                 on the 'FrameVelocityReperesentation' settings.                               
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  totalMomentum = getCentroidalTotalMomentum(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - totalMomentum: [6 x 1] vector of linear and angular momentum.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the momentum
+    totalMomentum_iDyntree = KinDynModel.kinDynComp.getCentroidalTotalMomentum();
+    
+    % convert to Matlab format
+    totalMomentum = totalMomentum_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m b/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m
new file mode 100644
index 00000000000..f67a974fd02
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFloatingBase.m
@@ -0,0 +1,25 @@
+function baseLinkName = getFloatingBase(KinDynModel)
+
+    % GETFLOATINGBASE retrieves the floating base link name from the 
+    %                      reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  baseLinkName = getFloatingBase(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - baseLinkName: name of the base link.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the name of the floating base link
+    baseLinkName = KinDynModel.kinDynComp.getFloatingBase();  
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m b/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m
new file mode 100644
index 00000000000..ef2c18dd694
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFrameBiasAcc.m
@@ -0,0 +1,29 @@
+function JDot_nu_frame = getFrameBiasAcc(KinDynModel,frameName)
+
+    % GETFRAMEBIASACC gets the bias accelerations of a specified frame. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  JDot_nu_frame = getFrameBiasAcc(KinDynModel,frameName)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - frameName: a string that specifies the frame w.r.t. compute the 
+    %                       bias accelerations, or the associated ID;     
+    %
+    % OUTPUTS: - JDot_nu_frame: [6 x 6+ndof] frame bias accelerations.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the bias acc
+    JDot_nu_frame_iDyntree = KinDynModel.kinDynComp.getFrameBiasAcc(frameName);
+    
+    % convert to Matlab format
+    JDot_nu_frame = JDot_nu_frame_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m b/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m
new file mode 100644
index 00000000000..4f6ce3636c5
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFrameFreeFloatingJacobian.m
@@ -0,0 +1,38 @@
+function J_frame = getFrameFreeFloatingJacobian(KinDynModel,frameName)
+
+    % GETFRAMEFREEFLOATINGJACOBIAN gets the free floating jacobian of a 
+    %                                   specified frame. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  J_frame = getFrameFreeFloatingJacobian(KinDynModel,frameName)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - frameName: a string that specifies the frame w.r.t. compute the 
+    %                       jacobian matrix, or the associated ID;     
+    %
+    % OUTPUTS: - J_frame: [6 x 6+ndof] frame free floating Jacobian.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the matrix that must be populated with the jacobian map
+    J_frame_iDyntree = iDynTree.MatrixDynSize(6,KinDynModel.NDOF+6);
+    
+    % get the free floating jacobian
+    ack = KinDynModel.kinDynComp.getFrameFreeFloatingJacobian(frameName,J_frame_iDyntree);
+    
+    % check for errors
+    if ~ack   
+        error('[getFrameFreeFloatingJacobian]: unable to get the Jacobian from the reduced model.')
+    end
+    
+    % convert to Matlab format
+    J_frame = J_frame_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m b/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m
new file mode 100644
index 00000000000..3eb11a769a0
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFrameIndex.m
@@ -0,0 +1,25 @@
+function frameID = getFrameIndex(KinDynModel,frameName)
+
+    % GETFRAMEINDEX gets the index corresponding to a given frame name. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  frameID = getFrameIndex(KinDynModel,frameName)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - frameName: a string specifying a valid frame name.
+    %
+    % OUTPUTS: - frameID: the ID associated to the given frame name.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the ID of the given frame
+    frameID = KinDynModel.kinDynComp.getFrameIndex(frameName); 
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameName.m b/bindings/matlab/+iDynTreeWrappers/getFrameName.m
new file mode 100644
index 00000000000..74aa02d0245
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFrameName.m
@@ -0,0 +1,25 @@
+function frameName = getFrameName(KinDynModel,frameID)
+
+    % GETFRAMENAME gets the name corresponding to a given frame index.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  frameName = getFrameName(KinDynModel,frameID)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - frameID: the ID associated to the given frame name.
+    %
+    % OUTPUTS: - frameName: a string specifying a valid frame name.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % get the ID of the given frame
+    frameName = KinDynModel.kinDynComp.getFrameName(frameID);   
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m b/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m
new file mode 100644
index 00000000000..918b167cf16
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFrameVelocityRepresentation.m
@@ -0,0 +1,52 @@
+function frameVelRepr = getFrameVelocityRepresentation(KinDynModel)
+
+    % GETFRAMEVELOCITYREPRESENTATION retrieves the current frame velocity
+    %                                     representation. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  frameVelRepr = getFrameVelocityRepresentation(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - frameVelRepr: a string with one of the following possible values:
+    %                          'mixed', 'body', 'inertial';
+    %
+    % Possible frame velocity representations:
+    %
+    %  0 = INERTIAL_FIXED_REPRESENTATION 	
+    %
+    %  1 = BODY_FIXED_REPRESENTATION
+    %
+    %  2 = MIXED_REPRESENTATION
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    frameVelRepr_idyntree = KinDynModel.kinDynComp.getFrameVelocityRepresentation();   
+    
+    % output the current frame velocity representation
+    switch frameVelRepr_idyntree  
+        
+        case 2
+            
+            frameVelRepr = 'mixed';
+                    
+        case 1
+            
+            frameVelRepr = 'body';
+                    
+        case 0
+            
+            frameVelRepr = 'inertial';
+                    
+        otherwise        
+            error('[setFrameVelocityRepresentation]: frameVelRepr is not a valid string.')
+    end   
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m b/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m
new file mode 100644
index 00000000000..3eb975d359d
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getFreeFloatingMassMatrix.m
@@ -0,0 +1,59 @@
+function M = getFreeFloatingMassMatrix(KinDynModel)
+
+    % GETFREEFLOATINGMASSMATRIX retrieves the free floating mass matrix.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  M = getFreeFloatingMassMatrix(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - M: [6+ndof x 6+ndof] free floating mass matrix.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the matrix that must be populated with the mass info
+    M_iDyntree = iDynTree.MatrixDynSize(KinDynModel.NDOF+6,KinDynModel.NDOF+6);
+    
+    % get the mass matrix
+    ack = KinDynModel.kinDynComp.getFreeFloatingMassMatrix(M_iDyntree);
+    
+    % check for errors
+    if ~ack  
+        error('[getFreeFloatingMassMatrix]: unable to retrieve the mass matrix from the reduced model.')
+    end
+    
+    % convert to Matlab format
+    M = M_iDyntree.toMatlab;
+    
+    % debug output
+    if KinDynModel.DEBUG
+        
+        disp('[getFreeFloatingMassMatrix]: debugging outputs...')
+                
+        % check mass matrix symmetry and positive definiteness
+        M_symm_error = norm(M -(M + M')/2);
+        M_symm_eig   = eig((M + M')/2);
+ 
+        if M_symm_error > 0.1
+            
+            error('[getFreeFloatingMassMatrix]: M is not symmetric')
+        end
+        
+        for k = 1:length(M_symm_eig)
+            
+            if M_symm_eig(k) < 0
+                
+                error('[getFreeFloatingMassMatrix]: M is not positive definite')
+            end
+        end       
+        disp('[getFreeFloatingMassMatrix]: done.')     
+    end 
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getJointPos.m b/bindings/matlab/+iDynTreeWrappers/getJointPos.m
new file mode 100644
index 00000000000..d4139be3caf
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getJointPos.m
@@ -0,0 +1,35 @@
+function jointPos = getJointPos(KinDynModel)
+
+    % GETJOINTPOS retrieves the joints configuration from the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  jointPos = getJointPos(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - jointPos: [ndof x 1] vector of joint positions.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the vector that must be populated with the joint positions
+    jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    
+    % get the joints positions
+    ack = KinDynModel.kinDynComp.getJointPos(jointPos_iDyntree);
+    
+    % check for errors
+    if ~ack   
+        error('[getJointPos]: unable to retrieve the joint positions from the reduced model.')
+    end
+    
+    % convert to Matlab format
+    jointPos = jointPos_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getJointVel.m b/bindings/matlab/+iDynTreeWrappers/getJointVel.m
new file mode 100644
index 00000000000..3dc0243315a
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getJointVel.m
@@ -0,0 +1,35 @@
+function jointVel = getJointVel(KinDynModel)
+
+    % GETJOINTVEL retrieves joint velocities from the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  jointVel = getJointVel(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - jointVel: [ndof x 1] vector of joint velocities.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % create the vector that must be populated with the joint velocities
+    jointVel_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    
+    % get the joints velocities
+    ack = KinDynModel.kinDynComp.getJointVel(jointVel_iDyntree);
+    
+    % check for errors
+    if ~ack   
+        error('[getJointVel]: unable to retrieve the joint velocities from the reduced model.')
+    end
+    
+    % convert to Matlab format
+    jointVel = jointVel_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getModelVel.m b/bindings/matlab/+iDynTreeWrappers/getModelVel.m
new file mode 100644
index 00000000000..487a16a79fb
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getModelVel.m
@@ -0,0 +1,36 @@
+function stateVel = getModelVel(KinDynModel)
+
+    % GETMODELVEL gets the joints and floating base velocities from the 
+    %                  reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  stateVel = getModelVel(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - stateVel: [6+ndof x 1] vector of joints and base velocities.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % create the vector that must be populated with the stae velocities
+    stateVel_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    
+    % get the joints velocities
+    ack = KinDynModel.kinDynComp.getModelVel(stateVel_iDyntree);
+    
+    % check for errors
+    if ~ack  
+        error('[getModelVel]: unable to retrieve the state velocities from the reduced model.')
+    end
+    
+    % convert to Matlab format
+    stateVel = stateVel_iDyntree.toMatlab;
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m b/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m
new file mode 100644
index 00000000000..a285b59bd93
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getNrOfDegreesOfFreedom.m
@@ -0,0 +1,38 @@
+function nDof = getNrOfDegreesOfFreedom(KinDynModel)
+
+    % GETNROFDEGREESOFFREEDOM gets the dimension of the joint space. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  nDof = getNrOfDegreesOfFreedom(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - nDof: number of DoFs of the system.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % get the number of DoF
+    nDof = KinDynModel.kinDynComp.getNrOfDegreesOfFreedom(); 
+    
+    % Debug output
+    if KinDynModel.DEBUG
+        
+        disp('[getNrOfDegreesOfFreedom]: debugging outputs...')
+        
+        % check nDof is not empty
+        if isempty(nDof)
+            
+            error('[getNrOfDegreesOfFreedom]: nDof is empty.')
+        end
+               
+        disp('[getNrOfDegreesOfFreedom]: done.')     
+    end
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m b/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m
new file mode 100644
index 00000000000..0d798adc74e
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getRelativeJacobian.m
@@ -0,0 +1,41 @@
+function J_frameVel = getRelativeJacobian(KinDynModel,frameVelID,frameRefID)
+
+    % GETRELATIVEJACOBIAN gets the relative jacobian, i.e. the matrix that
+    %                          maps the velocity of frameVel expressed w.r.t.
+    %                          frameRef, to the joint velocity. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  J_frameVel = getRelativeJacobian(KinDynModel,frameVelID,frameRefID)
+    %
+    % INPUTS:  - frameRefID: a number that specifies the frame w.r.t. the velocity
+    %                        of frameVel is expressed;
+    %          - frameVelID: a number that specifies the frame whose velocity
+    %                        is the one mapped by the jacobian;
+    %          - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - J_frameVel: [6 x ndof] frameVel Jacobian.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % create the matrix that must be populated with the jacobian map
+    J_frameVel_iDyntree = iDynTree.MatrixDynSize(6,KinDynModel.NDOF);
+    
+    % get the relative jacobian
+    ack = KinDynModel.kinDynComp.getRelativeJacobian(frameVelID,frameRefID,J_frameVel_iDyntree);  
+    
+    % check for errors
+    if ~ack  
+        error('[getRelativeJacobian]: unable to get the relative jacobian from the reduced model.')
+    end
+    
+    % covert to Matlab format
+    J_frameVel = J_frameVel_iDyntree.toMatlab; 
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m b/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m
new file mode 100644
index 00000000000..63e847ab88f
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getRelativeTransform.m
@@ -0,0 +1,93 @@
+function frame1_H_frame2 = getRelativeTransform(KinDynModel,frame1Name,frame2Name)
+
+    % GETRELATIVETRANSFORM gets the transformation matrix between two specified 
+    %                           frames.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  frame1_H_frame2 = getRelativeTransform(KinDynModel,frame1Name,frame2Name)
+    %
+    % INPUTS:  - frame1Name: a string that specifies the frame w.r.t. compute the 
+    %                        transfomation matrix, or the associated ID;  
+    %          - frame2Name: a string that specifies the frame w.r.t. compute the 
+    %                        transfomation matrix, or the associated ID;   
+    %          - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - frame1_H_frame2: [4 x 4] from frame2 to frame1 transformation matrix.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the transformation between the frame 1 and 2
+    frame1_H_frame2_iDyntree = KinDynModel.kinDynComp.getRelativeTransform(frame1Name,frame2Name);  
+    frame1_R_frame2_iDyntree = frame1_H_frame2_iDyntree.getRotation;
+    framePos_iDyntree        = frame1_H_frame2_iDyntree.getPosition;
+    
+    % covert to Matlab format
+    frame1_R_frame2          = frame1_R_frame2_iDyntree.toMatlab;
+    framePos                 = framePos_iDyntree.toMatlab;
+    frame1_H_frame2          = [frame1_R_frame2,framePos;
+                                    0,   0,   0,   1];  
+                              
+    % Debug output
+    if KinDynModel.DEBUG
+        
+        disp('[getRelativeTransform]: debugging outputs...')
+
+        % frame1_H_frame2 must be a valid transformation matrix
+        if size(frame1_H_frame2,1) ~= 4 || size(frame1_H_frame2,2) ~= 4
+            
+           error('[getRelativeTransform]: frame1_H_frame2 is not a 4x4 matrix.')
+        end
+        
+        for ii = 1:4
+                    
+           if ii < 4
+                        
+               if abs(frame1_H_frame2(4,ii)) > 0.0001 
+            
+                   error('[getRelativeTransform]: the last line of frame1_H_frame2 is not [0,0,0,1].')
+               end
+           else
+               if abs(frame1_H_frame2(4,ii)) > 1.0001 || abs(frame1_H_frame2(4,ii)) < 0.9999
+            
+                   error('[getRelativeTransform]: the last line of frame1_H_frame2 is not [0,0,0,1].')
+               end
+           end
+        end
+        
+        % frame1_R_frame2 = frame1_H_frame2(1:3,1:3) must be a valid rotation matrix
+        if det(frame1_H_frame2(1:3,1:3)) < 0.9 || det(frame1_H_frame2(1:3,1:3)) > 1.1
+            
+            error('[getRelativeTransform]: frame1_R_frame2 is not a valid rotation matrix.')
+        end
+       
+        IdentityMatr = frame1_H_frame2(1:3,1:3)*frame1_H_frame2(1:3,1:3)';
+        
+        for kk = 1:size(IdentityMatr, 1)
+            
+            for jj = 1:size(IdentityMatr, 1)
+                
+                if jj == kk
+                    
+                    if abs(IdentityMatr(kk,jj)) < 0.9 || abs(IdentityMatr(kk,jj)) > 1.1
+                        
+                        error('[getRelativeTransform]: frame1_R_frame2 is not a valid rotation matrix.')
+                    end
+                else
+                    if abs(IdentityMatr(kk,jj)) > 0.01
+                        
+                        error('[getRelativeTransform]: frame1_R_frame2 is not a valid rotation matrix.')
+                    end
+                end
+            end   
+        end                                       
+        disp('[getRelativeTransform]: done.')     
+    end
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getRobotState.m b/bindings/matlab/+iDynTreeWrappers/getRobotState.m
new file mode 100644
index 00000000000..23f6b1d28a6
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getRobotState.m
@@ -0,0 +1,81 @@
+function [basePose,jointPos,baseVel,jointVel] = getRobotState(KinDynModel)
+
+    % GETROBOTSTATE gets the floating base system state from the reduced model.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  [basePose,jointPos,baseVel,jointVel] = getRobotState(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - basePose: [4 x 4] from base frame to world frame transform;
+    %          - jointPos: [ndof x 1] vector of joint positions;
+    %          - baseVel: [6 x 1] vector of base velocity;
+    %          - jointVel: [ndof x 1] vector of joint velocities.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % define all the iDyntree required quantities
+    basePose_iDyntree   = iDynTree.Transform();
+    jointPos_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    baseVel_iDyntree    = iDynTree.Twist();
+    jointVel_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    gravityVec_iDyntree = iDynTree.Vector3();
+    
+    % get the floating base state
+    KinDynModel.kinDynComp.getRobotState(basePose_iDyntree,jointPos_iDyntree,baseVel_iDyntree,jointVel_iDyntree,gravityVec_iDyntree);
+   
+    % get the base position and orientation
+    baseRotation_iDyntree = basePose_iDyntree.getRotation;
+    baseOrigin_iDyntree   = basePose_iDyntree.getPosition;
+    
+    % covert to Matlab format
+    baseRotation = baseRotation_iDyntree.toMatlab;
+    baseOrigin   = baseOrigin_iDyntree.toMatlab;
+    basePose     = [baseRotation, baseOrigin;
+                       0,  0,  0,  1]; 
+    jointPos     = jointPos_iDyntree.toMatlab;
+    baseVel      = baseVel_iDyntree.toMatlab;
+    jointVel     = jointVel_iDyntree.toMatlab;   
+    
+    % Debug output
+    if KinDynModel.DEBUG
+        
+        disp('[getRobotState]: debugging outputs...')
+        
+        % baseRotation = basePose(1:3,1:3) must be a valid rotation matrix
+        if det(basePose(1:3,1:3)) < 0.9 || det(basePose(1:3,1:3)) > 1.1
+            
+            error('[getRobotState]: baseRotation is not a valid rotation matrix.')
+        end
+        
+        IdentityMatr = basePose(1:3,1:3)*basePose(1:3,1:3)';
+        
+        for kk = 1:size(IdentityMatr, 1)
+            
+            for jj = 1:size(IdentityMatr, 1)
+                
+                if jj == kk
+                    
+                    if abs(IdentityMatr(kk,jj)) < 0.9 || abs(IdentityMatr(kk,jj)) > 1.1
+                        
+                        error('[getRobotState]: baseRotation is not a valid rotation matrix.')
+                    end
+                else
+                    if abs(IdentityMatr(kk,jj)) > 0.01
+                        
+                        error('[getRobotState]: baseRotation is not a valid rotation matrix.')
+                    end
+                end
+            end   
+        end    
+        disp('[getRobotState]: done.')     
+    end
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m
new file mode 100644
index 00000000000..c762f168a83
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getWorldBaseTransform.m
@@ -0,0 +1,67 @@
+function basePose = getWorldBaseTransform(KinDynModel)
+
+    % GETWORLDBASETRANSFORM gets the transformation matrix between the base frame
+    %                            and the inertial reference frame. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  basePose = getWorldBaseTransform(KinDynModel)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - basePose: [4 x 4] from base to world transformation matrix.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the transformation between the base and the world frames 
+    basePose_iDyntree     = KinDynModel.kinDynComp.getWorldBaseTransform();  
+    baseRotation_iDyntree = basePose_iDyntree.getRotation;
+    baseOrigin_iDyntree   = basePose_iDyntree.getPosition;
+    
+    % covert to Matlab format
+    baseRotation          = baseRotation_iDyntree.toMatlab;
+    baseOrigin            = baseOrigin_iDyntree.toMatlab;
+    basePose              = [baseRotation, baseOrigin;
+                                0,   0,   0,   1];                  
+   % Debug output
+    if KinDynModel.DEBUG
+        
+        disp('[getWorldBaseTransform]: debugging outputs...')
+        
+        % baseRotation = basePose(1:3,1:3) must be a valid rotation matrix
+        if det(basePose(1:3,1:3)) < 0.9 || det(basePose(1:3,1:3)) > 1.1
+            
+            error('[getWorldBaseTransform]: baseRotation is not a valid rotation matrix.')
+        end
+        
+        IdentityMatr = basePose(1:3,1:3)*basePose(1:3,1:3)';
+        
+        for kk = 1:size(IdentityMatr, 1)
+            
+            for jj = 1:size(IdentityMatr, 1)
+                
+                if jj == kk
+                    
+                    if abs(IdentityMatr(kk,jj)) < 0.9 || abs(IdentityMatr(kk,jj)) > 1.1
+                        
+                        error('[getWorldBaseTransform]: baseRotation is not a valid rotation matrix.')
+                    end
+                else
+                    if abs(IdentityMatr(kk,jj)) > 0.01
+                        
+                        error('[getWorldBaseTransform]: baseRotation is not a valid rotation matrix.')
+                    end
+                end
+            end   
+        end       
+             
+        disp('[getWorldBaseTransform]: done.')     
+    end   
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m
new file mode 100644
index 00000000000..c3a1a745929
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/getWorldTransform.m
@@ -0,0 +1,68 @@
+function w_H_frame = getWorldTransform(KinDynModel,frameName)
+
+    % GETWORLDTRANSFORM gets the transformation matrix between a specified 
+    %                        frame and the inertial reference frame.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  w_H_frame = getWorldTransform(KinDynModel,frameName)
+    %
+    % INPUTS:  - frameName: a string that specifies the frame w.r.t. compute the 
+    %                       transfomation matrix, or the associated ID;     
+    %          - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % OUTPUTS: - w_H_frame: [4 x 4] from frame to world transformation matrix.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % get the transformation between the frame and the world 
+    w_H_frame_iDyntree = KinDynModel.kinDynComp.getWorldTransform(frameName);  
+    w_R_frame_iDyntree = w_H_frame_iDyntree.getRotation;
+    framePos_iDyntree  = w_H_frame_iDyntree.getPosition;
+    
+    % covert to Matlab format
+    w_R_frame          = w_R_frame_iDyntree.toMatlab;
+    framePos           = framePos_iDyntree.toMatlab;
+    w_H_frame          = [w_R_frame, framePos;
+                           0,   0,   0,   1];                     
+    % Debug output
+    if KinDynModel.DEBUG
+        
+        disp('[getWorldTransform]: debugging outputs...')
+        
+         % w_R_frame must be a valid rotation matrix
+         if det(w_R_frame) < 0.9 || det(w_R_frame) > 1.1
+            
+             error('[getWorldTransform]: w_R_frame is not a valid rotation matrix.')
+         end
+        
+         IdentityMatr = w_H_frame(1:3,1:3)*w_H_frame(1:3,1:3)';
+        
+         for kk = 1:size(IdentityMatr, 1)
+            
+             for jj = 1:size(IdentityMatr, 1)
+                
+                 if jj == kk
+                    
+                     if abs(IdentityMatr(kk,jj)-1) > 0.9
+                        
+                         error('[getWorldTransform]: w_R_frame is not a valid rotation matrix.')
+                     end
+                 else
+                     if abs(IdentityMatr(kk,jj)) > 0.1
+                        
+                          error('[getWorldTransform]: w_R_frame is not a valid rotation matrix.')
+                     end
+                 end
+             end   
+         end                          
+         disp('[getWorldTransform]: done.')     
+    end
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m b/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m
new file mode 100644
index 00000000000..08c36a533b2
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/initializeVisualizer.m
@@ -0,0 +1,42 @@
+function Visualizer = initializeVisualizer(KinDynModel,debugMode)
+
+    % INITIALIZEVISUALIZER opens the iDyntree visualizer and loads the reduced
+    %                           model into the visualizer.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON).
+    %
+    % FORMAT:  Visualizer = initializeVisualizer(KinDynModel,debugMode)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - debugMode: if TRUE, the visualizer is used in "debug" mode;
+    %
+    % OUTPUTS: - Visualizer: a structure containing the visualizer and its options.
+    % 
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    Visualizer.viz   = iDynTree.Visualizer();
+    
+    % load the model in the visualizer
+    ack = Visualizer.viz.addModel(KinDynModel.kinDynComp.model(),'viz1');
+    
+    % check for errors
+    if ~ack   
+        error('[initializeVisualizer]: unable to load the model in the visualizer.')
+    end  
+
+    % draw the model
+    Visualizer.viz.draw();
+    
+    % if DEBUG option is set to TRUE, all the wrappers related to the
+    % iDyntree visualizer will be run in DEBUG mode.
+    Visualizer.DEBUG = debugMode;    
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m b/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m
new file mode 100644
index 00000000000..c21b3c8622d
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/loadReducedModel.m
@@ -0,0 +1,63 @@
+function KinDynModel = loadReducedModel(jointList,baseLinkName,modelPath,modelName,debugMode)
+
+    % LOADREDUCEDMODEL loads the urdf model of the rigid multi-body system.
+    %                     
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT:  KinDynModel = loadReducedModel(jointList,baseLinkName,modelPath,modelName,debugMode)
+    %
+    % INPUTS:  - jointList: cell array containing the list of joints to be used
+    %                       in the reduced model;
+    %          - baseLinkName: a string that specifies link which is considered
+    %                          as the floating base;
+    %          - modelPath: a string that specifies the path to the urdf model;
+    %          - modelName: a string that specifies the model name;
+    %          - debugMode: if TRUE, the wrappers are used in "debug" mode;
+    %
+    % OUTPUTS: - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % Author: Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    disp(['[loadReducedModel]: loading the following model: ',[modelPath,modelName]]);
+        
+    % if DEBUG option is set to TRUE, all the wrappers will be run in debug
+    % mode. Wrappers concerning iDyntree simulator have their own debugger
+    KinDynModel.DEBUG      = debugMode;
+    
+    % retrieve the link that will be used as the floating base
+    KinDynModel.BASE_LINK  = baseLinkName;
+        
+    % load the list of joints to be used in the reduced model
+    jointList_idyntree     = iDynTree.StringVector();
+    
+    for k = 1:length(jointList)
+        
+        jointList_idyntree.push_back(jointList{k});
+    end
+
+    % only joints specified in the joint list will be considered in the model
+    modelLoader            = iDynTree.ModelLoader();
+    reducedModel           = modelLoader.model();
+
+    modelLoader.loadReducedModelFromFile([modelPath,modelName], jointList_idyntree);
+
+    % get the number of degrees of freedom of the reduced model
+    KinDynModel.NDOF       = reducedModel.getNrOfDOFs();
+
+    % initialize the iDyntree KinDynComputation class, that will be used for
+    % computing the floating base system state, dynamics, and kinematics
+    KinDynModel.kinDynComp = iDynTree.KinDynComputations();
+
+    KinDynModel.kinDynComp.loadRobotModel(reducedModel);
+    
+    % set the floating base link
+    KinDynModel.kinDynComp.setFloatingBase(KinDynModel.BASE_LINK);
+    
+    disp(['[loadReducedModel]: loaded model: ',[modelPath,modelName],', number of joints: ',num2str(KinDynModel.NDOF)]);
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m b/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m
new file mode 100644
index 00000000000..783f44e0b73
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/setFloatingBase.m
@@ -0,0 +1,29 @@
+function [] = setFloatingBase(KinDynModel,floatBaseLinkName)
+
+    % SETFLOATINGBASE sets the link that is used as floating base.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT: [] = setFloatingBase(KinDynModel,floatBaseLinkName)
+    %
+    % INPUTS: - floatBaseLinkName: a string with the name of the link to be
+    %                              used as floating base;
+    %         - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % set the floating base link
+    ack = KinDynModel.kinDynComp.setFloatingBase(floatBaseLinkName);
+    
+    % check for errors
+    if ~ack  
+        error('[setFloatingBase]: unable to set the floating base link.')
+    end  
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m b/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m
new file mode 100644
index 00000000000..b1c24bf7e94
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/setFrameVelocityRepresentation.m
@@ -0,0 +1,55 @@
+function [] = setFrameVelocityRepresentation(KinDynModel,frameVelRepr)
+
+    % SETFRAMEVELOCITYREPRESENTATION sets the frame velocity representation.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT: [] = setFrameVelocityRepresentation(KinDynModel,frameVelRepr)
+    %
+    % INPUTS:  - frameVelRepr: a string with one of the following values:
+    %                          'mixed', 'body', 'inertial';
+    %          - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % Mapping for the frame velocity reperesentation
+    %
+    %  0 = INERTIAL_FIXED_REPRESENTATION
+    %
+    %  1 = BODY_FIXED_REPRESENTATION
+    %
+    %  2 = MIXED_REPRESENTATION
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    switch frameVelRepr
+        
+        case 'mixed'
+            
+            frameVelRepr_idyntree = iDynTree.MIXED_REPRESENTATION;
+                    
+        case 'body'
+            
+            frameVelRepr_idyntree = iDynTree.BODY_FIXED_REPRESENTATION;
+                    
+        case 'inertial'
+            
+            frameVelRepr_idyntree = iDynTree.INERTIAL_FIXED_REPRESENTATION;
+                    
+        otherwise        
+            error('[setFrameVelocityRepresentation]: frameVelRepr is not a valid string.')
+    end
+             
+    % set the desired frameVelRepr
+    ack = KinDynModel.kinDynComp.setFrameVelocityRepresentation(frameVelRepr_idyntree);
+    
+    % check for errors
+    if ~ack    
+        error('[setFrameVelocityRepresentation]: unable to set the frame velocity representation.')
+    end
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/setJointPos.m b/bindings/matlab/+iDynTreeWrappers/setJointPos.m
new file mode 100644
index 00000000000..1afa351ac68
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/setJointPos.m
@@ -0,0 +1,52 @@
+function [] = setJointPos(KinDynModel,jointPos)
+
+    % SETJOINTPOS sets the joints configuration for kino-dynamic 
+    %                  computations.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT: [] = setJointPos(KinDynModel,jointPos)
+    %
+    % INPUTS: - jointPos: [ndof x 1] vector representing the joints 
+    %                     configuration in radians;
+    %         - KinDynModel: a structure containing the loaded model and additional info.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % Debug input
+    if KinDynModel.DEBUG
+        
+        disp('[setJointPos]: debugging inputs...')
+        
+        % check joints position vector size
+        if length(jointPos) ~= KinDynModel.NDOF
+            
+            error('[setJointPos]: the length of jointPos is not KinDynModel.NDOF')
+        end
+            
+        disp('[setJointPos]: done.')     
+    end
+    
+    % convert the joint position to a dynamic size vector
+    jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    
+    for k = 0:length(jointPos)-1
+        
+        jointPos_iDyntree.setVal(k,jointPos(k+1));
+    end
+    
+    % set the current joint positions
+    ack = KinDynModel.kinDynComp.setJointPos(jointPos_iDyntree);
+    
+    % check for errors
+    if ~ack  
+        error('[setJointPos]: unable to set the joint positions.')
+    end  
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/setRobotState.m b/bindings/matlab/+iDynTreeWrappers/setRobotState.m
new file mode 100644
index 00000000000..4e15dd90873
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/setRobotState.m
@@ -0,0 +1,233 @@
+function [] = setRobotState(varargin)
+
+    % SETROBOTSTATE sets the system state. The system state is composed of:
+    %                   
+    %                    - joints configuration and velocity plus gravity vector 
+    %                      for fixed-base systems; 
+    %                    - base pose and velocity, joints configuration and 
+    %                      velocity plus gravity vector for floating-base systems. 
+    %
+    %                    The gravity vector expresses the gravity acceleration 
+    %                    in the inertial frame. 
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % FORMAT: Floating base system:
+    %
+    %         [] = setRobotState(KinDynModel,basePose,jointPos,baseVel,jointVel,gravAcc)
+    %
+    %         Fixed base system:
+    %
+    %         [] = setRobotState(KinDynModel,jointPos,jointVel,gravAcc)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - basePose: [4 x 4] from base frame to world frame transform;
+    %          - jointPos: [nDof x 1] vector representing the joints configuration 
+    %                      in radians;
+    %          - baseVel: [6 x 1] vector of base velocities [lin, ang];
+    %          - jointVel: [ndof x 1] vector of joints velocities;
+    %          - gravAcc: [3 x 1] vector of the gravity acceleration in the
+    %                             inertial frame.
+    %
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    KinDynModel = varargin{1};
+    
+    % check the number of inputs
+    switch nargin
+       
+        case 6
+            
+            basePose = varargin{2};
+            jointPos = varargin{3};
+            baseVel  = varargin{4};
+            jointVel = varargin{5};
+            gravAcc  = varargin{6};
+            
+            % Debug inputs
+            if KinDynModel.DEBUG
+        
+                disp('[setRobotState]: debugging inputs...')
+        
+                % basePose must be a valid transformation matrix
+                if size(basePose,1) ~= 4 || size(basePose,2) ~= 4
+            
+                    error('[setRobotState]: basePose is not a 4x4 matrix.')
+                end
+        
+                for ii = 1:4
+                    
+                    if ii < 4
+                        
+                        if abs(basePose(4,ii)) > 0.0001 
+            
+                            error('[setRobotState]: the last line of basePose is not [0,0,0,1].')
+                        end
+                    else
+                        if abs(basePose(4,ii)) > 1.0001 || abs(basePose(4,ii)) < 0.9999
+            
+                            error('[setRobotState]: the last line of basePose is not [0,0,0,1].')
+                        end
+                    end
+                end
+        
+                % baseRotation = basePose(1:3,1:3) must be a valid rotation matrix
+                if det(basePose(1:3,1:3)) < 0.9 || det(basePose(1:3,1:3)) > 1.1
+            
+                    error('[setRobotState]: baseRotation is not a valid rotation matrix.')
+                end
+        
+                IdentityMatr = basePose(1:3,1:3)*basePose(1:3,1:3)';
+        
+                for kk = 1:size(IdentityMatr, 1)
+            
+                    for jj = 1:size(IdentityMatr, 1)
+                
+                        if jj == kk
+                    
+                            if abs(IdentityMatr(kk,jj)) < 0.9 || abs(IdentityMatr(kk,jj)) > 1.1
+                        
+                                error('[setRobotState]: baseRotation is not a valid rotation matrix.')
+                            end
+                        else
+                            if abs(IdentityMatr(kk,jj)) > 0.01
+                        
+                                error('[setRobotState]: baseRotation is not a valid rotation matrix.')
+                            end
+                        end
+                    end   
+                end   
+            
+                % debug gravity vector size
+                if length(gravAcc) ~= 3
+            
+                     error('[setRobotState]: the length of gravAcc vector is not 3.')
+                end 
+                
+                % check base velocity vector size
+                if length(baseVel) ~= 6
+            
+                     error('[setRobotState]: the length of baseVel is not 6.')
+                end
+                
+                % check joints position vector size
+                if length(jointPos) ~= KinDynModel.NDOF
+            
+                    error('[setRobotState]: the length of jointPos is not KinDynModel.NDOF')
+                end
+                
+                % check joints velocity vector size
+                if length(jointVel) ~= KinDynModel.NDOF
+            
+                    error('[setRobotState]: the length of jointVel is not KinDynModel.NDOF')
+                end
+                
+                disp('[setRobotState]: done.')     
+            end
+            
+            % define the quantities required to set the floating base
+            baseRotation_iDyntree = iDynTree.Rotation();
+            baseOrigin_iDyntree   = iDynTree.Position();
+            basePose_iDyntree     = iDynTree.Transform();
+            baseVel_iDyntree      = iDynTree.Twist();
+            
+            % set the element of the rotation matrix and of the base
+            % position vector
+            for k = 0:2
+                
+                baseOrigin_iDyntree.setVal(k,basePose(k+1,4));
+                
+                for j = 0:2
+                    
+                    baseRotation_iDyntree.setVal(k,j,basePose(k+1,j+1));                   
+                end
+            end      
+            
+            % add the rotation matrix and the position to basePose_iDyntree
+            basePose_iDyntree.setRotation(baseRotation_iDyntree);
+            basePose_iDyntree.setPosition(baseOrigin_iDyntree);
+            
+            % set the base velocities
+            for k = 0:5
+                
+                baseVel_iDyntree.setVal(k,baseVel(k+1));
+            end
+            
+        case 4
+            
+            jointPos = varargin{2}; 
+            jointVel = varargin{3};
+            gravAcc  = varargin{4};
+            
+            % Debug inputs
+            if KinDynModel.DEBUG
+        
+                disp('[setRobotState]: debugging inputs...')
+                    
+                % debug gravity vector
+                if length(gravAcc) ~= 3
+            
+                     error('[setRobotState]: the length of gravAcc vector is not 3.')
+                end
+                
+                % check joints position vector size
+                if length(jointPos) ~= KinDynModel.NDOF
+            
+                    error('[setRobotState]: the length of jointPos is not KinDynModel.NDOF')
+                end
+                
+                % check joints velocity vector size
+                if length(jointVel) ~= KinDynModel.NDOF
+            
+                    error('[setRobotState]: the length of jointVel is not KinDynModel.NDOF')
+                end
+                
+                disp('[setRobotState]: done.')     
+            end
+            
+        otherwise        
+            error('[setRobotState]: wrong number of inputs.')
+    end
+    
+    % define all the remaining quantities required for setting the system state
+    jointPos_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    jointVel_iDyntree   = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    gravityVec_iDyntree = iDynTree.Vector3();
+         
+    % set joints position and velocity
+    for k = 0:length(jointPos)-1
+        
+        jointVel_iDyntree.setVal(k,jointVel(k+1));
+        jointPos_iDyntree.setVal(k,jointPos(k+1));
+    end
+    
+    % set the gravity vector
+    for k = 0:2
+        
+       gravityVec_iDyntree.setVal(k,gravAcc(k+1));       
+    end
+    
+    % set the current robot state
+    switch nargin
+        
+        case 4
+            
+            ack = KinDynModel.kinDynComp.setRobotState(jointPos_iDyntree,jointVel_iDyntree,gravityVec_iDyntree);
+            
+        case 6
+            
+            ack = KinDynModel.kinDynComp.setRobotState(basePose_iDyntree,jointPos_iDyntree,baseVel_iDyntree,jointVel_iDyntree,gravityVec_iDyntree);
+    end
+    
+    % check for errors
+    if ~ack   
+        error('[setRobotState]: unable to set the robot state.')
+    end  
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m b/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m
new file mode 100644
index 00000000000..fd6af5478e7
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/updateVisualizer.m
@@ -0,0 +1,127 @@
+function [] = updateVisualizer(Visualizer,KinDynModel,jointPos,basePose)
+
+    % UPDATEVISUALIZER updates the iDyntree visualizer with the current
+    %                  base pose and joints position.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON).
+    %
+    % FORMAT: [] = updateVisualizer(Visualizer,KinDynModel,jointPos,basePose)
+    %
+    % INPUTS:  - KinDynModel: a structure containing the loaded model and additional info.
+    %          - Visualizer: a structure containing the visualizer and its options.
+    %          - jointPos: [ndof x 1] vector representing the joints
+    %                      configuration in radians;
+    %          - basePose: [4 x 4] from base frame to world frame transform.
+    % 
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+
+    % Debug input
+    if Visualizer.DEBUG
+        
+        disp('[updateVisualizer]: debugging inputs...')
+        
+        % basePose must be a valid transformation matrix
+        if size(basePose,1) ~= 4 || size(basePose,2) ~= 4
+            
+            error('[updateVisualizer]: basePose is not a 4x4 matrix.')
+        end
+        
+        for ii = 1:4
+                    
+            if ii < 4
+                        
+                if abs(basePose(4,ii)) > 0.0001 
+            
+                    error('[updateVisualizer]: the last line of basePose is not [0,0,0,1].')
+                end
+            else
+                if abs(basePose(4,ii)) > 1.0001 || abs(basePose(4,ii)) < 0.9999
+            
+                    error('[updateVisualizer]: the last line of basePose is not [0,0,0,1].')
+                end
+            end
+        end
+        
+        % baseRotation = basePose(1:3,1:3) must be a valid rotation matrix
+        if det(basePose(1:3,1:3)) < 0.9 || det(basePose(1:3,1:3)) > 1.1
+            
+            error('[updateVisualizer]: baseRotation is not a valid rotation matrix.')
+        end
+        
+        IdentityMatr = basePose(1:3,1:3)*basePose(1:3,1:3)';
+        
+        for kk = 1:size(IdentityMatr, 1)
+            
+            for jj = 1:size(IdentityMatr, 1)
+                
+                if jj == kk
+                    
+                    if abs(IdentityMatr(kk,jj)) < 0.9 || abs(IdentityMatr(kk,jj)) > 1.1
+                        
+                        error('[updateVisualizer]: baseRotation is not a valid rotation matrix.')
+                    end
+                else
+                    if abs(IdentityMatr(kk,jj)) > 0.01
+                        
+                        error('[updateVisualizer]: baseRotation is not a valid rotation matrix.')
+                    end
+                end
+            end   
+        end   
+        
+        % check joint position vector size
+        if length(jointPos) ~= KinDynModel.NDOF
+            
+            error('[updateVisualizer]: the length of jointPos is not KinDynModel.NDOF')
+        end
+            
+        disp('[updateVisualizer]: done.')     
+    end
+    
+    % convert joints position to a dynamic size vector
+    jointPos_iDyntree = iDynTree.VectorDynSize(KinDynModel.NDOF);
+    
+    for k = 0:length(jointPos)-1
+        
+        jointPos_iDyntree.setVal(k,jointPos(k+1));
+    end
+    
+    % define the quantities required to set the floating base pose
+    baseRotation_iDyntree = iDynTree.Rotation();
+    baseOrigin_iDyntree   = iDynTree.Position();
+    basePose_iDyntree     = iDynTree.Transform();
+            
+    % set the elements of the rotation matrix and of the base position vector
+    for k = 0:2
+                
+        baseOrigin_iDyntree.setVal(k,basePose(k+1,4));
+                
+        for j = 0:2
+                    
+            baseRotation_iDyntree.setVal(k,j,basePose(k+1,j+1));                   
+        end
+    end
+            
+    % add the rotation matrix and the position to w_H_b_iDyntree
+    basePose_iDyntree.setRotation(baseRotation_iDyntree);
+    basePose_iDyntree.setPosition(baseOrigin_iDyntree);
+
+    % set the current joints position and world-to-base transform
+    ack = Visualizer.viz.modelViz(0).setPositions(basePose_iDyntree,jointPos_iDyntree);
+       
+    % check for errors
+    if ~ack    
+        error('[updateVisualizer]: unable to update the visualizer.')
+    end
+    
+    Visualizer.viz.draw();
+end
diff --git a/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m b/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m
new file mode 100644
index 00000000000..12633657af0
--- /dev/null
+++ b/bindings/matlab/+iDynTreeWrappers/visualizerSetup.m
@@ -0,0 +1,53 @@
+function [] = visualizerSetup(Visualizer,disableViewInertialFrame,lightDir,cameraPos,cameraTarget)
+
+    % VISUALIZERSETUP modifies the visualization environment according to the 
+    %                      user specifications.
+    %
+    % This matlab function wraps a functionality of the iDyntree library.                     
+    % For further info see also: https://github.com/robotology/idyntree
+    %
+    % REQUIREMENTS: compile iDyntree with Irrlicht (IDYNTREE_USES_IRRLICHT = ON).
+    %
+    % FORMAT:  [] = visualizerSetup(Visualizer,disableViewInertialFrame,lightDir,cameraPos,cameraTarget)
+    %
+    % INPUTS:  - Visualizer: a structure containing the visualizer and its options.
+    %          - disableViewInertialFrame: boolean for disabling the view of the 
+    %                                      inertial frame;
+    %          - lightDir: [3 x 1] vector describing the light direction;
+    %          - cameraPos: [3 x 1] vector describing the camera position;
+    %          - cameraTarget: [3 x 1] vector describing the camera target;
+    % 
+    % Author : Gabriele Nava (gabriele.nava@iit.it)
+    %
+    % Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+    % This software may be modified and distributed under the terms of the
+    % GNU Lesser General Public License v2.1 or any later version.
+
+    %% ------------Initialization----------------
+    
+    % disable environmental features  
+    if disableViewInertialFrame
+        
+        enviroment = Visualizer.viz.enviroment();  
+        ack        = enviroment.setElementVisibility('root_frame',false);   
+        
+        % check for errors
+        if ~ack
+            error('[visualizerSetup]: unable to disable the inertial frame view.')
+        end         
+    end
+    
+    % set lights
+    sun = Visualizer.viz.enviroment().lightViz('sun');     
+    lightDir_idyntree = iDynTree.Direction();     
+    lightDir_idyntree.fromMatlab(lightDir); 
+    sun.setDirection(lightDir_idyntree);
+
+    % set camera     
+    cam = Visualizer.viz.camera();     
+    cam.setPosition(iDynTree.Position(cameraPos(1),cameraPos(2),cameraPos(3)));     
+    cam.setTarget(iDynTree.Position(cameraTarget(1),cameraTarget(2),cameraTarget(3)));    
+    
+    % draw the model
+    Visualizer.viz.draw();
+end
diff --git a/bindings/matlab/CMakeLists.txt b/bindings/matlab/CMakeLists.txt
index f7f9bcf8213..5e80f3dc221 100644
--- a/bindings/matlab/CMakeLists.txt
+++ b/bindings/matlab/CMakeLists.txt
@@ -61,6 +61,10 @@ set(mexname    iDynTreeMEX)
 # Directory in which the bindings are generated
 set(MEX_BINDINGS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/autogenerated)
 
+# Directory containing the high-level-matlab-wrappers and its parent directory
+set(HIGH_LEVEL_WRAPPERS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/+iDynTreeWrappers)
+set(MATLAB_WRAPPERS_BINDINGS_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR})
+
 # Generate SWIG wrapper
 if(IDYNTREE_GENERATE_MATLAB)
   # generate the wrapper
@@ -78,7 +82,7 @@ endif()
 set(swig_generated_sources ${MEX_BINDINGS_SOURCE_DIR}/${sourcename}.cxx)
 set(swig_other_sources)
 
-# Set the generated mex name to be iDynTreeMEX, as it the defaul one used by SWIG while generating bindings
+# Set the generated mex name to be iDynTreeMEX, as it is the default one used by SWIG while generating bindings
 if(IDYNTREE_USES_MATLAB)
   find_package(Matlab
                REQUIRED
@@ -104,6 +108,9 @@ if(IDYNTREE_USES_MATLAB)
   install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigMem.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_MFILESDIR})
   install(TARGETS ${mexname} DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_LIBDIR})
 
+  # Install the high-level-matlab-wrappers
+  install(DIRECTORY ${HIGH_LEVEL_WRAPPERS_SOURCE_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_MFILESDIR})
+
   #On new versions of Clang, MATLAB requires C++11.
   #I enable it on all Clangs
   if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
@@ -143,6 +150,9 @@ if(IDYNTREE_USES_OCTAVE)
   install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigGet.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_OCTAVE_MFILESDIR})
   install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigRef.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_OCTAVE_MFILESDIR})
   install(FILES ${MEX_BINDINGS_SOURCE_DIR}/SwigMem.m DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_OCTAVE_MFILESDIR})
+
+  # Install the high-level-matlab-wrappers
+  install(DIRECTORY ${HIGH_LEVEL_WRAPPERS_SOURCE_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}/${IDYNTREE_INSTALL_MATLAB_MFILESDIR})
 endif()
 
 # if compile tests compile also matlab/octave tests
diff --git a/bindings/matlab/tests/CMakeLists.txt b/bindings/matlab/tests/CMakeLists.txt
index 38c10e2d6e0..dcb44266b41 100644
--- a/bindings/matlab/tests/CMakeLists.txt
+++ b/bindings/matlab/tests/CMakeLists.txt
@@ -8,10 +8,10 @@ add_custom_command(TARGET copy_matlab_model_in_build PRE_BUILD
 
 if (IDYNTREE_USES_MATLAB)
   add_test(NAME matlab_idyntree_tests
-           COMMAND ${Matlab_MAIN_PROGRAM} -nodisplay -nodesktop -nojvm -r "addpath('$<TARGET_FILE_DIR:${mexname}>');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);")
+           COMMAND ${Matlab_MAIN_PROGRAM} -nodisplay -nodesktop -nojvm -r "addpath('$<TARGET_FILE_DIR:${mexname}>');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${MATLAB_WRAPPERS_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);")
 endif()
 
 if (IDYNTREE_USES_OCTAVE)
   add_test(NAME octave_idyntree_tests
-           COMMAND ${OCTAVE_EXECUTABLE} --no-gui --quiet --eval "addpath('$<TARGET_FILE_DIR:idyntreeOctaveMex>');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);")
-endif()
\ No newline at end of file
+           COMMAND ${OCTAVE_EXECUTABLE} --no-gui --quiet --eval "addpath('$<TARGET_FILE_DIR:idyntreeOctaveMex>');addpath('${MEX_BINDINGS_SOURCE_DIR}');addpath('${MATLAB_WRAPPERS_BINDINGS_SOURCE_DIR}');addpath('${CMAKE_CURRENT_SOURCE_DIR}/');addpath(genpath('${IDYNTREE_INTERNAL_MOXUNIT_PATH}'));success=moxunit_runtests('${CMAKE_CURRENT_SOURCE_DIR}','-verbose');exit(~success);")
+endif()
diff --git a/bindings/matlab/tests/highLevelWrappersSmokeTest.m b/bindings/matlab/tests/highLevelWrappersSmokeTest.m
new file mode 100644
index 00000000000..293bfd37315
--- /dev/null
+++ b/bindings/matlab/tests/highLevelWrappersSmokeTest.m
@@ -0,0 +1,155 @@
+function test_suite = highLevelWrappersSmokeTest
+    initTestSuite
+
+function test__high_level_wrappers
+   
+    % specify the list of joints that are going to be considered in the reduced model
+    jointList    = {'torso_pitch','torso_roll','torso_yaw',...
+                    'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow','l_wrist_prosup', ...
+                    'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow','r_wrist_prosup', ...
+                    'l_hip_pitch','l_hip_roll','l_hip_yaw','l_knee','l_ankle_pitch','l_ankle_roll', ...
+                    'r_hip_pitch','r_hip_roll','r_hip_yaw','r_knee','r_ankle_pitch','r_ankle_roll'};
+        
+    % select the link that will be used as base link
+    baseLinkName = 'root_link';
+
+    % urdf model name (expected to be in the same folder of this test)
+    modelName    = 'model.urdf';
+
+    % set the initial robot position and velocity [deg]
+    torso_Position     = [0  0  0];                 
+    left_arm_Position  = [10 45 0  15 0];           
+    right_arm_Position = [10 45 0  15 0];         
+    right_leg_Position = [0  0  0  0  0  0];        
+    left_leg_Position  = [0  0  0  0  0  0];
+
+    jointPos_init = [torso_Position';left_arm_Position';right_arm_Position';left_leg_Position';right_leg_Position']*pi/180;
+    jointVel_init = zeros(length(jointPos_init),1);
+
+    % other configuration parameters
+    gravityAcc    = [0;0;-9.81];
+    frameVelRepr  = 'mixed';
+    frameName     = 'l_sole';
+    frame2Name    = 'r_sole';
+    frameID       = 1;
+    frame2ID      = 2;
+          
+    %% TESTS LIST
+    try
+        % test 1
+        disp('1/28: testing loadReducedModel...')
+        KinDynModel = iDynTreeWrappers.loadReducedModel(jointList, baseLinkName, './', modelName, true);
+
+        % test 2
+        disp('2/28: testing setRobotState...')
+        iDynTreeWrappers.setRobotState(KinDynModel, jointPos_init, jointVel_init, gravityAcc)
+
+        % test 3
+        disp('3/28: testing setJointPos...')
+        iDynTreeWrappers.setJointPos(KinDynModel, jointPos_init) 
+
+        % test 4
+        disp('4/28: testing setFrameVelocityRepresentation...')
+        iDynTreeWrappers.setFrameVelocityRepresentation(KinDynModel, frameVelRepr)
+
+        % test 5
+        disp('5/28: testing setFloatingBase...')
+        iDynTreeWrappers.setFloatingBase(KinDynModel, baseLinkName)
+ 
+        % test 6
+        disp('6/28: testing getJointPos...')
+        iDynTreeWrappers.getJointPos(KinDynModel);
+
+        % test 7
+        disp('7/28: testing getJointVel...')
+        iDynTreeWrappers.getJointVel(KinDynModel);
+
+        % test 8
+        disp('8/28: testing getCentroidalTotalMomentum...')
+        iDynTreeWrappers.getCentroidalTotalMomentum(KinDynModel);
+    
+        % test 9
+        disp('9/28: testing getNrOfDegreesOfFreedom...')
+        iDynTreeWrappers.getNrOfDegreesOfFreedom(KinDynModel);
+
+        % test 10
+        disp('10/28: testing getCenterOfMassPosition...')
+        iDynTreeWrappers.getCenterOfMassPosition(KinDynModel);
+
+        % test 11
+        disp('11/28: testing getBaseTwist...')
+        iDynTreeWrappers.getBaseTwist(KinDynModel);
+
+        % test 12
+        disp('12/28: testing generalizedBiasForces...')
+        iDynTreeWrappers.generalizedBiasForces(KinDynModel);
+
+        % test 13
+        disp('13/28: testing generalizedGravityForces...')
+        iDynTreeWrappers.generalizedGravityForces(KinDynModel);
+
+        % test 14
+        disp('14/28: testing getWorldBaseTransform...')
+        iDynTreeWrappers.getWorldBaseTransform(KinDynModel);
+
+        % test 15
+        disp('15/28: testing getModelVel...')
+        iDynTreeWrappers.getModelVel(KinDynModel);
+    
+        % test 16
+        disp('16/28: testing getFrameVelocityRepresentation...')
+        iDynTreeWrappers.getFrameVelocityRepresentation(KinDynModel);
+
+        % test 17
+        disp('17/28: testing getFloatingBase...')
+        iDynTreeWrappers.getFloatingBase(KinDynModel);
+
+        % test 18
+        disp('18/28: testing getFrameIndex...')
+        iDynTreeWrappers.getFrameIndex(KinDynModel, frameName);
+
+        % test 19
+        disp('19/28: testing getFrameName...')
+        iDynTreeWrappers.getFrameName(KinDynModel, frameID);
+
+        % test 20
+        disp('20/28: testing getWorldTransform...')
+        iDynTreeWrappers.getWorldTransform(KinDynModel, frameName);
+
+        % test 21
+        disp('21/28: testing getRelativeTransform...')
+        iDynTreeWrappers.getRelativeTransform(KinDynModel, frameName, frame2Name);
+
+        % test 22
+        disp('22/28: testing getRelativeJacobian...')
+        iDynTreeWrappers.getRelativeJacobian(KinDynModel, frameID, frame2ID);
+
+        % test 23
+        disp('23/28: testing getFreeFloatingMassMatrix...')
+        iDynTreeWrappers.getFreeFloatingMassMatrix(KinDynModel);
+
+        % test 24
+        disp('24/28: testing getRobotState...')
+        iDynTreeWrappers.getRobotState(KinDynModel);
+
+        % test 25
+        disp('25/28: testing getFrameBiasAcc...')
+        iDynTreeWrappers.getFrameBiasAcc(KinDynModel, frameName);
+
+        % test 26
+        disp('26/28: testing getCenterOfMassJacobian...')
+        iDynTreeWrappers.getCenterOfMassJacobian(KinDynModel);
+
+        % test 27
+        disp('27/28: testing getCenterOfMassVelocity...')
+        iDynTreeWrappers.getCenterOfMassVelocity(KinDynModel);
+        
+        % test 28
+        disp('28/28: testing getFrameFreeFloatingJacobian...')
+        iDynTreeWrappers.getFrameFreeFloatingJacobian(KinDynModel, frameName);
+        
+    catch ME
+       disp('[High Level Wappers]: test failed. Message: ')
+       disp(ME)
+       assertTrue(false);
+    end
diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 8c1fd314669..e6183cda32d 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -42,6 +42,9 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
 
+#### `bindings`
+* Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)
+
 ### `core`
 * Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)
 * Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)

From a95d4a4df54846ca3efc61913e8ec4a737c42864 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 6 Feb 2019 15:33:33 +0100
Subject: [PATCH 103/194] Added methods to specify the constraints hessian in
 the MultipleShootingSolver.

---
 .../include/iDynTree/Constraint.h             |  54 ++++
 .../include/iDynTree/ConstraintsGroup.h       |  54 ++++
 .../include/iDynTree/DynamicalSystem.h        |  52 ++++
 .../include/iDynTree/Integrator.h             |  24 ++
 .../iDynTree/Integrators/ForwardEuler.h       |  12 +-
 .../Integrators/ImplicitTrapezoidal.h         |  10 +
 .../include/iDynTree/OptimalControlProblem.h  |  18 ++
 src/optimalcontrol/src/Constraint.cpp         |  29 +-
 src/optimalcontrol/src/ConstraintsGroup.cpp   | 189 ++++++++++++
 src/optimalcontrol/src/DynamicalSystem.cpp    |  45 +--
 src/optimalcontrol/src/ForwardEuler.cpp       |  92 +++++-
 .../src/ImplicitTrapezoidal.cpp               | 107 +++++++
 src/optimalcontrol/src/Integrator.cpp         |  37 ++-
 .../src/MultipleShootingSolver.cpp            | 284 ++++++++++++++++--
 .../src/OptimalControlProblem.cpp             | 116 +++++++
 15 files changed, 1057 insertions(+), 66 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/Constraint.h b/src/optimalcontrol/include/iDynTree/Constraint.h
index 4161ccaffb1..e01c1ee2e20 100644
--- a/src/optimalcontrol/include/iDynTree/Constraint.h
+++ b/src/optimalcontrol/include/iDynTree/Constraint.h
@@ -181,6 +181,60 @@ namespace iDynTree {
              */
             virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
 
+            /**
+             * @brief Evaluate constraint second partial derivative wrt the state variables
+             *
+             * It is the result of \f$\sum \lambda_i \frac{\partial^2 c(t, x, u)}{\partial x^2}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed..
+             * @param[in] lambda The lagrange multipliers
+             * @param[out] hessian The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
+            virtual bool constraintSecondPartialDerivativeWRTState(double time,
+                                                                   const VectorDynSize& state,
+                                                                   const VectorDynSize& control,
+                                                                   const VectorDynSize& lambda,
+                                                                   MatrixDynSize& hessian);
+
+
+            /**
+             * @brief Evaluate constraint second partial derivative wrt the control
+             *
+             * It is the result of \f$\sum \lambda_i \frac{\partial^2 c(t, x, u)}{\partial u^2}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[in] lambda The lagrange multipliers
+             * @param[out] hessian The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
+            virtual bool constraintSecondPartialDerivativeWRTControl(double time,
+                                                                     const VectorDynSize& state,
+                                                                     const VectorDynSize& control,
+                                                                     const VectorDynSize& lambda,
+                                                                     MatrixDynSize& hessian);
+
+
+            /**
+             * @brief Evaluate constraint second partial derivative wrt the state and control
+             *
+             * It is the result of \f$\sum \lambda_i \frac{\partial^2 c(t, x, u)}{\partial x \partial u}\f$,
+             * thus it has number of rows equals to the number of states and number of cols equal to the number of control inputs.
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[in] lambda The lagrange multipliers
+             * @param[out] hessian The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
+            virtual bool constraintSecondPartialDerivativeWRTStateControl(double time,
+                                                                          const VectorDynSize& state,
+                                                                          const VectorDynSize& control,
+                                                                          const VectorDynSize& lambda,
+                                                                          MatrixDynSize& hessian);
+
 
         private:
             size_t m_constraintSize;
diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index ccad846413a..c61b9266566 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -206,6 +206,60 @@ namespace iDynTree {
              */
             bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) const;
 
+            /**
+             * @brief Evaluate constraint second partial derivative wrt the state variables
+             *
+             * It is the result of \f$\sum \lambda_i \frac{\partial^2 c(t, x, u)}{\partial x^2}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed..
+             * @param[in] lambda The lagrange multipliers
+             * @param[out] hessian The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
+            bool constraintSecondPartialDerivativeWRTState(double time,
+                                                           const VectorDynSize& state,
+                                                           const VectorDynSize& control,
+                                                           const VectorDynSize& lambda,
+                                                           MatrixDynSize& hessian);
+
+
+            /**
+             * @brief Evaluate constraint second partial derivative wrt the control
+             *
+             * It is the result of \f$\sum \lambda_i \frac{\partial^2 c(t, x, u)}{\partial u^2}\f$
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[in] lambda The lagrange multipliers
+             * @param[out] hessian The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
+            bool constraintSecondPartialDerivativeWRTControl(double time,
+                                                             const VectorDynSize& state,
+                                                             const VectorDynSize& control,
+                                                             const VectorDynSize& lambda,
+                                                             MatrixDynSize& hessian);
+
+
+            /**
+             * @brief Evaluate constraint second partial derivative wrt the state and control
+             *
+             * It is the result of \f$\sum \lambda_i \frac{\partial^2 c(t, x, u)}{\partial x \partial u}\f$,
+             * thus it has number of rows equals to the number of states and number of cols equal to the number of control inputs.
+             * @param[in] time The time at which the partial derivative is computed.
+             * @param[in] state The state value at which the partial derivative is computed.
+             * @param[in] control The control value at which the partial derivative is computed.
+             * @param[in] lambda The lagrange multipliers
+             * @param[out] hessian The output partial derivative.
+             * @return True if successfull, false otherwise (or if not implemented).
+             */
+            bool constraintSecondPartialDerivativeWRTStateControl(double time,
+                                                                  const VectorDynSize& state,
+                                                                  const VectorDynSize& control,
+                                                                  const VectorDynSize& lambda,
+                                                                  MatrixDynSize& hessian);
+
             /**
              * @brief Flag returning true if the group is an "AnyTime" group.
              * An "AnyTime" group contains only one constraint which is always enabled. It corresponds to a simple constraint.
diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index 5754e41fa1e..4463a671316 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -185,6 +185,58 @@ namespace optimalcontrol {
          */
         virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
 
+        /**
+         * @brief Evaluate the dynamics second partial derivative wrt the state variables
+         *
+         * It is the result of \f$\frac{\partial^2 f(t, x, u)}{\partial x^2}\f$
+         * @param[in] time The time at which the partial derivative is computed.
+         * @param[in] state The state value at which the partial derivative is computed.
+         * @param[in] control The control value at which the partial derivative is computed.
+         * @param[in] lambda The associated lagrange multipliers
+         * @param[out] partialDerivative The output partial derivative.
+         * @return True if successfull, false otherwise (or if not implemented).
+         */
+        virtual bool dynamicsSecondPartialDerivativeWRTState(double time,
+                                                             const iDynTree::VectorDynSize& state,
+                                                             const iDynTree::VectorDynSize& control,
+                                                             const VectorDynSize& lambda,
+                                                             iDynTree::MatrixDynSize& partialDerivative);
+
+        /**
+         * @brief Evaluate the dynamics second partial derivative wrt the control
+         *
+         * It is the result of \f$\frac{\partial^2 f(t, x, u)}{\partial u^2}\f$
+         * @param[in] time The time at which the partial derivative is computed.
+         * @param[in] state The state value at which the partial derivative is computed.
+         * @param[in] control The control value at which the partial derivative is computed.
+         * @param[in] lambda The associated lagrange multipliers
+         * @param[out] partialDerivative The output partial derivative.
+         * @return True if successfull, false otherwise (or if not implemented).
+         */
+        virtual bool dynamicsSecondPartialDerivativeWRTControl(double time,
+                                                               const iDynTree::VectorDynSize& state,
+                                                               const iDynTree::VectorDynSize& control,
+                                                               const VectorDynSize& lambda,
+                                                               iDynTree::MatrixDynSize& partialDerivative);
+
+        /**
+         * @brief Evaluate the dynamics second partial derivative wrt the state and control
+         *
+         * It is the result of \f$\frac{\partial^2 f(t, x, u)}{\partial x \partial u}\f$,
+         * thus it has number of rows equals to the number of states and number of cols equal to the number of control inputs.
+         * @param[in] time The time at which the partial derivative is computed.
+         * @param[in] state The state value at which the partial derivative is computed.
+         * @param[in] control The control value at which the partial derivative is computed.
+         * @param[in] lambda The associated lagrange multipliers
+         * @param[out] partialDerivative The output partial derivative.
+         * @return True if successfull, false otherwise (or if not implemented).
+         */
+        virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double time,
+                                                                    const iDynTree::VectorDynSize& state,
+                                                                    const iDynTree::VectorDynSize& control,
+                                                                    const VectorDynSize& lambda,
+                                                                    iDynTree::MatrixDynSize& partialDerivative);
+
     private:
         size_t m_stateSize;
         size_t m_controlSize;
diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index 7b48080d1cb..eac8d4a6a30 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -23,6 +23,7 @@
 #include <vector>
 #include <string>
 #include <memory>
+#include <map>
 
 namespace iDynTree {
 
@@ -105,6 +106,22 @@ namespace optimalcontrol {
                 size_t numberOfStages() const;
             };
 
+
+            class CollocationHessianIndex {
+                size_t m_first;
+                size_t m_second;
+
+            public:
+
+                CollocationHessianIndex() = delete;
+
+                CollocationHessianIndex(size_t first, size_t second);
+
+                bool operator< (const CollocationHessianIndex& rhs) const;
+            };
+
+            using CollocationHessianMap = std::map<CollocationHessianIndex, MatrixDynSize>;
+
             /**
              * @warning This class is still in active development, and so API interface can change between iDynTree versions.
              * \ingroup iDynTreeExperimental
@@ -239,6 +256,13 @@ namespace optimalcontrol {
                 virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure>& controlJacobianSparsity);
 
 
+                virtual bool evaluateCollocationConstraintSecondDerivatives(double time, const std::vector<VectorDynSize>& collocationPoints,
+                                                                            const std::vector<VectorDynSize>& controlInputs, double dT,
+                                                                            const VectorDynSize& lambda,
+                                                                            CollocationHessianMap& stateSecondDerivative,
+                                                                            CollocationHessianMap& controlSecondDerivative,
+                                                                            CollocationHessianMap& stateControlSecondDerivative);
+
                 const IntegratorInfo& info() const;
 
             protected:
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
index f04449c6ac9..650d7e80742 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
@@ -34,7 +34,10 @@ namespace iDynTree {
             class ForwardEuler : public FixedStepIntegrator{
 
                 VectorDynSize m_computationBuffer;
-                MatrixDynSize m_stateJacBuffer, m_controlJacBuffer, m_identity, m_zeroBuffer;
+                MatrixDynSize m_stateJacBuffer, m_controlJacBuffer;
+                MatrixDynSize m_identity, m_zeroNxNxBuffer, m_zeroNuNuBuffer, m_zeroNxNuBuffer;
+                MatrixDynSize m_stateHessianBuffer, m_controlHessianBuffer, m_mixedHessianBuffer;
+                VectorDynSize m_lambda;
                 bool m_hasStateSparsity = false;
                 bool m_hasControlSparsity = false;
                 std::vector<SparsityStructure> m_stateJacobianSparsity;
@@ -65,6 +68,13 @@ namespace iDynTree {
 
                 virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure>& controlJacobianSparsity) override;
 
+                virtual bool evaluateCollocationConstraintSecondDerivatives(double time, const std::vector<VectorDynSize>& collocationPoints,
+                                                                            const std::vector<VectorDynSize>& controlInputs, double dT,
+                                                                            const VectorDynSize& lambda,
+                                                                            CollocationHessianMap& stateSecondDerivative,
+                                                                            CollocationHessianMap& controlSecondDerivative,
+                                                                            CollocationHessianMap& stateControlSecondDerivative) override;
+
             };
         }
     }
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
index 4567fd1dba4..d26965523b1 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -35,6 +35,9 @@ namespace iDynTree {
 
                 VectorDynSize m_computationBuffer, m_computationBuffer2;
                 MatrixDynSize m_identity, m_stateJacBuffer, m_controlJacBuffer;
+                MatrixDynSize m_zeroNxNxBuffer, m_zeroNuNuBuffer, m_zeroNxNuBuffer;
+                MatrixDynSize m_stateHessianBuffer, m_controlHessianBuffer, m_mixedHessianBuffer;
+                VectorDynSize m_lambda;
                 bool m_hasStateSparsity = false;
                 bool m_hasControlSparsity = false;
                 std::vector<SparsityStructure> m_stateJacobianSparsity;
@@ -65,6 +68,13 @@ namespace iDynTree {
 
                 virtual bool getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure>& controlJacobianSparsity) override;
 
+                virtual bool evaluateCollocationConstraintSecondDerivatives(double time, const std::vector<VectorDynSize>& collocationPoints,
+                                                                            const std::vector<VectorDynSize>& controlInputs, double dT,
+                                                                            const VectorDynSize& lambda,
+                                                                            CollocationHessianMap& stateSecondDerivative,
+                                                                            CollocationHessianMap& controlSecondDerivative,
+                                                                            CollocationHessianMap& stateControlSecondDerivative) override;
+
             };
 
         }
diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index 085d97bae9e..aacc421f409 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -246,6 +246,24 @@ namespace iDynTree {
 
             bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
 
+            bool constraintSecondPartialDerivativeWRTState(double time,
+                                                           const VectorDynSize& state,
+                                                           const VectorDynSize& control,
+                                                           const VectorDynSize& lambda,
+                                                           MatrixDynSize& hessian);
+
+            bool constraintSecondPartialDerivativeWRTControl(double time,
+                                                             const VectorDynSize& state,
+                                                             const VectorDynSize& control,
+                                                             const VectorDynSize& lambda,
+                                                             MatrixDynSize& hessian);
+
+            bool constraintSecondPartialDerivativeWRTStateControl(double time,
+                                                                  const VectorDynSize& state,
+                                                                  const VectorDynSize& control,
+                                                                  const VectorDynSize& lambda,
+                                                                  MatrixDynSize& hessian);
+
         private:
             class OptimalControlProblemPimpl;
             OptimalControlProblemPimpl* m_pimpl;
diff --git a/src/optimalcontrol/src/Constraint.cpp b/src/optimalcontrol/src/Constraint.cpp
index 9bf756fa382..6909f03dcf0 100644
--- a/src/optimalcontrol/src/Constraint.cpp
+++ b/src/optimalcontrol/src/Constraint.cpp
@@ -14,8 +14,9 @@
  * - ADRL Control Toolbox (https://adrlab.bitbucket.io/ct/ct_doc/doc/html/index.html)
  */
 
-#include "iDynTree/Constraint.h"
-#include "iDynTree/Core/Utils.h"
+#include <iDynTree/Constraint.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/MatrixDynSize.h>
 
 #include <cassert>
 
@@ -113,12 +114,12 @@ namespace iDynTree {
             return true;
         }
 
-        bool Constraint::constraintJacobianWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &jacobian)
+        bool Constraint::constraintJacobianWRTState(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, MatrixDynSize &/*jacobian*/)
         {
             return false;
         }
 
-        bool Constraint::constraintJacobianWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &jacobian)
+        bool Constraint::constraintJacobianWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, MatrixDynSize &/*jacobian*/)
         {
             return false;
         }
@@ -133,12 +134,28 @@ namespace iDynTree {
             return 0;
         }
 
-        bool Constraint::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
+        bool Constraint::constraintJacobianWRTStateSparsity(SparsityStructure &/*stateSparsity*/)
         {
             return false;
         }
 
-        bool Constraint::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
+        bool Constraint::constraintJacobianWRTControlSparsity(SparsityStructure &/*controlSparsity*/)
+        {
+            return false;
+        }
+
+        bool Constraint::constraintSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const VectorDynSize &/*lambda*/, MatrixDynSize &/*hessian*/)
+        {
+            return false;
+        }
+
+
+        bool Constraint::constraintSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const VectorDynSize &/*lambda*/, MatrixDynSize &/*hessian*/)
+        {
+            return false;
+        }
+
+        bool Constraint::constraintSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const VectorDynSize &/*lambda*/, MatrixDynSize &/*hessian*/)
         {
             return false;
         }
diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index 0bbbd4ce39e..efb68833d50 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -38,6 +38,7 @@ namespace optimalcontrol {
             VectorDynSize constraintBuffer;
             MatrixDynSize stateJacobianBuffer;
             MatrixDynSize controlJacobianBuffer;
+            VectorDynSize lambdaBuffer;
         }TimedConstraint;
 
         typedef std::shared_ptr<TimedConstraint> TimedConstraint_ptr;
@@ -109,6 +110,8 @@ namespace optimalcontrol {
                 newConstraint->constraintBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()));
                 newConstraint->stateJacobianBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()), static_cast<unsigned int>(constraint->expectedStateSpaceSize()));
                 newConstraint->controlJacobianBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()), static_cast<unsigned int>(constraint->expectedControlSpaceSize()));
+                newConstraint->lambdaBuffer.resize(static_cast<unsigned int>(constraint->constraintSize()));
+
 
                 std::pair< GroupOfConstraintsMap::iterator, bool> result;
                 result = group.insert(GroupOfConstraintsMap::value_type(constraint->name(), newConstraint));
@@ -568,6 +571,192 @@ namespace optimalcontrol {
             return true;
         }
 
+        bool ConstraintsGroup::constraintSecondPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        {
+            if (isAnyTimeGroup()){
+                TimedConstraint_ptr loneConstraint = m_pimpl->group.begin()->second;
+                if (!(loneConstraint->constraint->constraintSecondPartialDerivativeWRTState(time, state, control, lambda, hessian))) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Failed to evaluate "<< loneConstraint->constraint->name() << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The second partial derivative WRT the state of constraint "<< loneConstraint->constraint->name() << " has a number of rows different from the size of the state." << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.cols() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The second partial derivative WRT the state of constraint "<< loneConstraint->constraint->name() << " has a number of columns different from the size of the state." << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+
+                return true;
+            }
+
+            hessian.resize(state.size(), state.size());
+
+            std::vector< TimedConstraint_ptr >::reverse_iterator constraintIterator = m_pimpl->findActiveConstraint(time);
+            if (constraintIterator == m_pimpl->orderedIntervals.rend()){ //no active constraint
+                toEigen(hessian).setZero();
+                return true;
+            }
+
+            toEigen(constraintIterator->get()->lambdaBuffer) = toEigen(lambda).topRows(constraintIterator->get()->lambdaBuffer.size());
+
+            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTState(time, state, control, lambda, hessian))) {
+                std::ostringstream errorMsg;
+                errorMsg << "Failed to evaluate "<< constraintIterator->get()->constraint->name() << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                return false;
+            }
+
+            if (hessian.rows() != state.size()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The second partial derivative WRT the state of constraint "<< constraintIterator->get()->constraint->name() << " has a number of rows different from the size of the state." << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                return false;
+            }
+
+            if (hessian.cols() != state.size()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The second partial derivative WRT the state of constraint "<< constraintIterator->get()->constraint->name() << " has a number of columns different from the size of the state." << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                return false;
+            }
+
+            return true;
+        }
+
+        bool ConstraintsGroup::constraintSecondPartialDerivativeWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        {
+            if (isAnyTimeGroup()){
+                TimedConstraint_ptr loneConstraint = m_pimpl->group.begin()->second;
+                if (!(loneConstraint->constraint->constraintSecondPartialDerivativeWRTControl(time, state, control, lambda, hessian))) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Failed to evaluate "<< loneConstraint->constraint->name() << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The second partial derivative WRT the control of constraint "<< loneConstraint->constraint->name() << " has a number of rows different from the size of the control." << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.cols() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The second partial derivative WRT the control of constraint "<< loneConstraint->constraint->name() << " has a number of columns different from the size of the control." << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                return true;
+            }
+
+            hessian.resize(control.size(), control.size());
+
+            std::vector< TimedConstraint_ptr >::reverse_iterator constraintIterator = m_pimpl->findActiveConstraint(time);
+            if (constraintIterator == m_pimpl->orderedIntervals.rend()){ //no active constraint
+                toEigen(hessian).setZero();
+                return true;
+            }
+
+            toEigen(constraintIterator->get()->lambdaBuffer) = toEigen(lambda).topRows(constraintIterator->get()->lambdaBuffer.size());
+
+            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTControl(time, state, control, lambda, hessian))) {
+                std::ostringstream errorMsg;
+                errorMsg << "Failed to evaluate "<< constraintIterator->get()->constraint->name() << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                return false;
+            }
+
+            if (hessian.rows() != control.size()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The second partial derivative WRT the control of constraint "<< constraintIterator->get()->constraint->name() << " has a number of rows different from the size of the control." << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                return false;
+            }
+
+            if (hessian.cols() != control.size()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The second partial derivative WRT the control of constraint "<< constraintIterator->get()->constraint->name() << " has a number of columns different from the size of the control." << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                return false;
+            }
+
+            return true;
+        }
+
+        bool ConstraintsGroup::constraintSecondPartialDerivativeWRTStateControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        {
+            if (isAnyTimeGroup()){
+                TimedConstraint_ptr loneConstraint = m_pimpl->group.begin()->second;
+                if (!(loneConstraint->constraint->constraintSecondPartialDerivativeWRTStateControl(time, state, control, lambda, hessian))) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Failed to evaluate "<< loneConstraint->constraint->name() << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.rows() != state.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The second partial derivative WRT state/control of constraint "<< loneConstraint->constraint->name() << " has a number of rows different from the size of the state." << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (hessian.cols() != control.size()) {
+                    std::ostringstream errorMsg;
+                    errorMsg << "The second partial derivative WRT state/control of constraint "<< loneConstraint->constraint->name() << " has a number of columns different from the size of the control." << std::endl;
+                    reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                    return false;
+                }
+
+                return true;
+            }
+
+            hessian.resize(state.size(), state.size());
+
+            std::vector< TimedConstraint_ptr >::reverse_iterator constraintIterator = m_pimpl->findActiveConstraint(time);
+            if (constraintIterator == m_pimpl->orderedIntervals.rend()){ //no active constraint
+                toEigen(hessian).setZero();
+                return true;
+            }
+
+            toEigen(constraintIterator->get()->lambdaBuffer) = toEigen(lambda).topRows(constraintIterator->get()->lambdaBuffer.size());
+
+            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTStateControl(time, state, control, lambda, hessian))) {
+                std::ostringstream errorMsg;
+                errorMsg << "Failed to evaluate "<< constraintIterator->get()->constraint->name() << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                return false;
+            }
+
+            if (hessian.rows() != state.size()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The second partial derivative WRT state/control of constraint "<< constraintIterator->get()->constraint->name() << " has a number of rows different from the size of the state." << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                return false;
+            }
+
+            if (hessian.cols() != control.size()) {
+                std::ostringstream errorMsg;
+                errorMsg << "The second partial derivative WRT state/control of constraint "<< constraintIterator->get()->constraint->name() << " has a number of columns different from the size of the control." << std::endl;
+                reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                return false;
+            }
+
+            return true;
+        }
+
         bool ConstraintsGroup::isAnyTimeGroup()
         {
             if ((numberOfConstraints() == 1) && (m_pimpl->group.begin()->second.get()->timeRange == TimeRange::AnyTime())) {
diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp
index 972a2b06ee8..8b56bd234ab 100644
--- a/src/optimalcontrol/src/DynamicalSystem.cpp
+++ b/src/optimalcontrol/src/DynamicalSystem.cpp
@@ -80,39 +80,42 @@ namespace iDynTree {
             return true;
         }
 
-        bool  DynamicalSystem::dynamicsStateFirstDerivative(const VectorDynSize& state,
-                                                            double time,
-                                                            MatrixDynSize& dynamicsDerivative)
+        bool  DynamicalSystem::dynamicsStateFirstDerivative(const VectorDynSize& /*state*/,
+                                                            double /*time*/,
+                                                            MatrixDynSize& /*dynamicsDerivative*/)
         { return false; }
 
-        bool DynamicalSystem::dynamicsControlFirstDerivative(const VectorDynSize& state,
-                                                             double time,
-                                                             MatrixDynSize& dynamicsDerivative)
+        bool DynamicalSystem::dynamicsControlFirstDerivative(const VectorDynSize& /*state*/,
+                                                             double /*time*/,
+                                                             MatrixDynSize& /*dynamicsDerivative*/)
         { return false; }
 
-        bool DynamicalSystem::dynamicsStateFirstDerivativeSparsity(SparsityStructure &stateSparsity)
+        bool DynamicalSystem::dynamicsStateFirstDerivativeSparsity(SparsityStructure &/*stateSparsity*/)
         {
             return false;
         }
 
-        bool DynamicalSystem::dynamicsControlFirstDerivativeSparsity(SparsityStructure &controlSparsity)
+        bool DynamicalSystem::dynamicsControlFirstDerivativeSparsity(SparsityStructure &/*controlSparsity*/)
         {
             return false;
         }
 
-//        bool DynamicalSystem::setController(std::shared_ptr<Controller> controllerPointer){
-//            if (controllerPointer->controlSpaceSize() != m_controlSize){
-//                reportError("DynamicalSystem", "setController", "The controller dimension is not coherent with the controlSpaceSize.");
-//                return false;
-//            }
-//            m_controller_ptr = controllerPointer;
-//            return true;
-//        }
-
-//        const std::weak_ptr<const Controller> DynamicalSystem::controller() const
-//        {
-//            return m_controller_ptr;
-//        }
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
+        {
+            return false;
+        }
+
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
+        {
+            return false;
+        }
+
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
+        {
+            return false;
+        }
+
+
 
     }
 }
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index f0c4be4aebf..97f7f1d68fa 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -44,8 +44,18 @@ namespace iDynTree {
                 m_stateJacBuffer.zero();
                 m_controlJacBuffer.resize(nx,nu);
                 m_controlJacBuffer.zero();
-                m_zeroBuffer.resize(nx,nu);
-                m_zeroBuffer.zero();
+                m_zeroNxNxBuffer.resize(nx,nx);
+                m_zeroNxNxBuffer.zero();
+                m_zeroNxNuBuffer.resize(nx,nu);
+                m_zeroNxNuBuffer.zero();
+                m_zeroNuNuBuffer.resize(nu,nu);
+                m_zeroNuNuBuffer.zero();
+
+                m_stateHessianBuffer.resize(nx, nx);
+                m_controlHessianBuffer.resize(nu, nu);
+                m_mixedHessianBuffer.resize(nx, nu);
+
+                m_lambda.resize(nx);
 
                 m_stateJacobianSparsity.resize(2);
                 m_controlJacobianSparsity.resize(2);
@@ -239,7 +249,7 @@ namespace iDynTree {
                     controlJacobianValues[1].resize(nx,nu);
                 }
 
-                controlJacobianValues[1] = m_zeroBuffer;
+                controlJacobianValues[1] = m_zeroNxNuBuffer;
 
                 return true;
             }
@@ -263,6 +273,82 @@ namespace iDynTree {
                 controlJacobianSparsity = m_controlJacobianSparsity;
                 return true;
             }
+
+            bool ForwardEuler::evaluateCollocationConstraintSecondDerivatives(double time, const std::vector<VectorDynSize> &collocationPoints,
+                                                                              const std::vector<VectorDynSize> &controlInputs, double dT,
+                                                                              const VectorDynSize &lambda, CollocationHessianMap &stateSecondDerivative,
+                                                                              CollocationHessianMap &controlSecondDerivative, CollocationHessianMap &stateControlSecondDerivative)
+            {
+                if (!m_dynamicalSystem_ptr){
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", "Dynamical system not set.");
+                    return false;
+                }
+
+                if (collocationPoints.size() != 2){
+                    std::ostringstream errorMsg;
+                    errorMsg << "The size of the matrix containing the collocation point does not match the expected one. Input = ";
+                    errorMsg << collocationPoints.size() << ", Expected = 2.";
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (controlInputs.size() != 2){
+                    std::ostringstream errorMsg;
+                    errorMsg << "The size of the matrix containing the control inputs does not match the expected one. Input = ";
+                    errorMsg << controlInputs.size() << ", Expected = 2.";
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (!((m_dynamicalSystem_ptr->setControlInput(controlInputs[0])))){
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintJacobian", "Error while setting the control input.");
+                    return false;
+                }
+
+                toEigen(m_lambda) = dT * toEigen(lambda);
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time, collocationPoints[0], controlInputs[0], m_lambda, m_stateHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system state second derivative.");
+                    return false;
+                }
+
+                stateSecondDerivative[CollocationHessianIndex(0, 0)] = m_stateHessianBuffer;
+
+                stateSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNxBuffer;
+
+                stateSecondDerivative[CollocationHessianIndex(1, 1)] = m_zeroNxNxBuffer;
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_controlHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system control second derivative.");
+                    return false;
+                }
+
+                controlSecondDerivative[CollocationHessianIndex(0, 0)] = m_controlHessianBuffer;
+
+                controlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNuNuBuffer;
+
+                controlSecondDerivative[CollocationHessianIndex(1, 1)] = m_zeroNuNuBuffer;
+
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_mixedHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system second derivative wrt state and control.");
+                    return false;
+                }
+
+                stateControlSecondDerivative[CollocationHessianIndex(0, 0)] = m_mixedHessianBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNuBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(1, 0)] = m_zeroNxNuBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(1, 1)] = m_zeroNxNuBuffer;
+
+
+                return true;
+            }
         }
     }
 }
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index d870495a924..96835b67629 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -50,6 +50,19 @@ namespace iDynTree {
                 m_stateJacobianSparsity.resize(2);
                 m_controlJacobianSparsity.resize(2);
 
+                m_zeroNxNxBuffer.resize(nx,nx);
+                m_zeroNxNxBuffer.zero();
+                m_zeroNxNuBuffer.resize(nx,nu);
+                m_zeroNxNuBuffer.zero();
+                m_zeroNuNuBuffer.resize(nu,nu);
+                m_zeroNuNuBuffer.zero();
+
+                m_stateHessianBuffer.resize(nx, nx);
+                m_controlHessianBuffer.resize(nu, nu);
+                m_mixedHessianBuffer.resize(nx, nu);
+
+                m_lambda.resize(nx);
+
                 if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0])) {
 
                     for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
@@ -272,6 +285,100 @@ namespace iDynTree {
                 return true;
             }
 
+            bool ImplicitTrapezoidal::evaluateCollocationConstraintSecondDerivatives(double time, const std::vector<VectorDynSize> &collocationPoints,
+                                                                                     const std::vector<VectorDynSize> &controlInputs, double dT, const VectorDynSize &lambda,
+                                                                                     iDynTree::optimalcontrol::integrators::CollocationHessianMap &stateSecondDerivative,
+                                                                                     iDynTree::optimalcontrol::integrators::CollocationHessianMap &controlSecondDerivative,
+                                                                                     iDynTree::optimalcontrol::integrators::CollocationHessianMap &stateControlSecondDerivative)
+            {
+                if (!m_dynamicalSystem_ptr){
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", "Dynamical system not set.");
+                    return false;
+                }
+
+                if (collocationPoints.size() != 2){
+                    std::ostringstream errorMsg;
+                    errorMsg << "The size of the matrix containing the collocation point does not match the expected one. Input = ";
+                    errorMsg << collocationPoints.size() << ", Expected = 2.";
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (controlInputs.size() != 2){
+                    std::ostringstream errorMsg;
+                    errorMsg << "The size of the matrix containing the control inputs does not match the expected one. Input = ";
+                    errorMsg << controlInputs.size() << ", Expected = 2.";
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", errorMsg.str().c_str());
+                    return false;
+                }
+
+                if (!((m_dynamicalSystem_ptr->setControlInput(controlInputs[0])))){
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintJacobian", "Error while setting the control input.");
+                    return false;
+                }
+
+                toEigen(m_lambda) = 0.5 * dT * toEigen(lambda);
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time, collocationPoints[0], controlInputs[0], m_lambda, m_stateHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system state second derivative.");
+                    return false;
+                }
+
+                stateSecondDerivative[CollocationHessianIndex(0, 0)] = m_stateHessianBuffer;
+
+                stateSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNxBuffer;
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time + dT, collocationPoints[1], controlInputs[1], m_lambda, m_stateHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system state second derivative.");
+                    return false;
+                }
+
+                stateSecondDerivative[CollocationHessianIndex(1, 1)] = m_stateHessianBuffer;
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_controlHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system control second derivative.");
+                    return false;
+                }
+
+                controlSecondDerivative[CollocationHessianIndex(0, 0)] = m_controlHessianBuffer;
+
+                controlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNuNuBuffer;
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time + dT, collocationPoints[1], controlInputs[1], m_lambda, m_controlHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system control second derivative.");
+                    return false;
+                }
+
+                controlSecondDerivative[CollocationHessianIndex(1, 1)] = m_controlHessianBuffer;
+
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_mixedHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system second derivative wrt state and control.");
+                    return false;
+                }
+
+                stateControlSecondDerivative[CollocationHessianIndex(0, 0)] = m_mixedHessianBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNuBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(1, 0)] = m_zeroNxNuBuffer;
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time + dT, collocationPoints[1], controlInputs[1], m_lambda, m_mixedHessianBuffer))) {
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
+                                "Error while evaluating the dynamical system second derivative wrt state and control.");
+                    return false;
+                }
+
+                stateControlSecondDerivative[CollocationHessianIndex(1, 1)] = m_mixedHessianBuffer;
+
+                return true;
+            }
+
         }
     }
 }
diff --git a/src/optimalcontrol/src/Integrator.cpp b/src/optimalcontrol/src/Integrator.cpp
index 3ee3c50e0f2..4bc1c8ba781 100644
--- a/src/optimalcontrol/src/Integrator.cpp
+++ b/src/optimalcontrol/src/Integrator.cpp
@@ -115,26 +115,37 @@ namespace iDynTree {
                 m_solution.clear();
             }
 
-            bool Integrator::evaluateCollocationConstraint(double time, const std::vector<VectorDynSize> &collocationPoints,
-                                                           const std::vector<VectorDynSize> &controlInputs, double dT, VectorDynSize &constraintValue)
+            bool Integrator::evaluateCollocationConstraint(double /*time*/, const std::vector<VectorDynSize> &/*collocationPoints*/,
+                                                           const std::vector<VectorDynSize> &/*controlInputs*/, double /*dT*/, VectorDynSize &/*constraintValue*/)
             {
                 return false;
             }
 
-            bool Integrator::evaluateCollocationConstraintJacobian(double time, const std::vector<VectorDynSize> &collocationPoints,
-                                                                   const std::vector<VectorDynSize> &controlInputs, double dT,
-                                                                   std::vector<MatrixDynSize> &stateJacobianValues,
-                                                                   std::vector<MatrixDynSize> &controlJacobianValues)
+            bool Integrator::evaluateCollocationConstraintJacobian(double /*time*/, const std::vector<VectorDynSize> &/*collocationPoints*/,
+                                                                   const std::vector<VectorDynSize> &/*controlInputs*/, double /*dT*/,
+                                                                   std::vector<MatrixDynSize> &/*stateJacobianValues*/,
+                                                                   std::vector<MatrixDynSize> &/*controlJacobianValues*/)
             {
                 return false;
             }
 
-            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &stateJacobianSparsity)
+            bool Integrator::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &/*stateJacobianSparsity*/)
             {
                 return false;
             }
 
-            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &controlJacobianSparsity)
+            bool Integrator::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &/*controlJacobianSparsity*/)
+            {
+                return false;
+            }
+
+            bool iDynTree::optimalcontrol::integrators::Integrator::evaluateCollocationConstraintSecondDerivatives(double /*time*/,
+                                                                                                                   const std::vector<VectorDynSize> &/*collocationPoints*/,
+                                                                                                                   const std::vector<VectorDynSize> &/*controlInputs*/,
+                                                                                                                   double /*dT*/, const VectorDynSize &/*lambda*/,
+                                                                                                                   CollocationHessianMap &/*stateSecondDerivative*/,
+                                                                                                                   CollocationHessianMap &/*controlSecondDerivative*/,
+                                                                                                                   CollocationHessianMap &/*stateControlSecondDerivative*/)
             {
                 return false;
             }
@@ -171,6 +182,16 @@ namespace iDynTree {
 
             size_t IntegratorInfo::numberOfStages() const {return m_data->numberOfStages;}
 
+            CollocationHessianIndex::CollocationHessianIndex(size_t first, size_t second)
+                : m_first(first)
+                  , m_second(second)
+            { }
+
+            bool CollocationHessianIndex::operator< (const CollocationHessianIndex& rhs) const {
+
+                return (m_first < rhs.m_first) || ((m_first == rhs.m_first) && (m_second < rhs.m_second));
+            }
+
         }
 
     }
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 4f1eba6b7e6..d8d7984637c 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -34,6 +34,7 @@
 #include <cmath>
 #include <string>
 #include <sstream>
+#include <map>
 
 namespace iDynTree {
     namespace optimalcontrol
@@ -85,6 +86,35 @@ namespace iDynTree {
         } MeshPoint;
 
         //MARK: Transcription implementation
+        class HessianBlocksMap {
+            std::map<CollocationHessianIndex, bool> m_map;
+
+        public:
+
+            HessianBlocksMap () { }
+
+            bool& operator()(size_t row, size_t col) {
+                CollocationHessianIndex indices(row, col);
+                auto it = m_map.find(indices);
+
+                if (it == m_map.end()) {
+                    m_map[indices] = false;
+                }
+                return m_map[indices];
+            }
+
+            void clear() {
+                m_map.clear();
+            }
+
+            void reset() {
+                for (auto& it : m_map) {
+                    it.second = false;
+                }
+            }
+
+        };
+
 
         class MultipleShootingSolver::MultipleShootingTranscription : public optimization::OptimizationProblem {
 
@@ -109,6 +139,10 @@ namespace iDynTree {
             std::vector<VectorDynSize> m_collocationStateBuffer, m_collocationControlBuffer;
             std::vector<MatrixDynSize> m_collocationStateJacBuffer, m_collocationControlJacBuffer;
             MatrixDynSize m_constraintsStateJacBuffer, m_constraintsControlJacBuffer;
+            VectorDynSize m_lambdaConstraints, m_lambdaCollocation;
+            MatrixDynSize m_stateHessianBuffer, m_controlHessianBuffer, m_stateControlHessianBuffer;
+            CollocationHessianMap m_stateCollocationHessians, m_controlCollocationHessians, m_stateControCollocationlHessians;
+            HessianBlocksMap m_hessianBlocksSet;
             VectorDynSize m_solution;
             std::shared_ptr<TimeVaryingVector> m_stateGuesses, m_controlGuesses;
             bool m_solved;
@@ -205,13 +239,54 @@ namespace iDynTree {
             }
 
             void addHessianBlock(size_t initRow, size_t rows, size_t initCol, size_t cols){
-                for (size_t i = 0; i < rows; ++i){
-                    for (size_t j = 0; j < cols; ++j){
-                        addNonZero(m_hessianNZRows, m_hessianNonZeros, initRow + i);
-                        addNonZero(m_hessianNZCols, m_hessianNonZeros, initCol + j);
-                        m_hessianNonZeros++;
+                if (!m_hessianBlocksSet(initRow, initCol)) {
+                    for (size_t i = 0; i < rows; ++i){
+                        for (size_t j = 0; j < cols; ++j){
+                            addNonZero(m_hessianNZRows, m_hessianNonZeros, initRow + i);
+                            addNonZero(m_hessianNZCols, m_hessianNonZeros, initCol + j);
+                            m_hessianNonZeros++;
+                        }
                     }
+
+                    m_hessianBlocksSet(initRow, initCol) = true;
                 }
+
+                //add also the transpose
+
+                if (!m_hessianBlocksSet(initCol, initRow)) {
+                    for (size_t i = 0; i < cols; ++i){
+                        for (size_t j = 0; j < rows; ++j){
+                            addNonZero(m_hessianNZRows, m_hessianNonZeros, initCol + i);
+                            addNonZero(m_hessianNZCols, m_hessianNonZeros, initRow + j);
+                            m_hessianNonZeros++;
+                        }
+                    }
+
+                    m_hessianBlocksSet(initCol, initRow) = true;
+                }
+
+            }
+
+            void setHessianBlock(MatrixDynSize& hessian, const MatrixDynSize& block, size_t startRow, size_t startCol) {
+                if (m_hessianBlocksSet(startRow, startCol)) {
+                    toEigen(hessian).block(static_cast<Eigen::Index>(startRow), static_cast<unsigned int>(startCol), block.rows(), block.cols()) += toEigen(block);
+                } else {
+                    toEigen(hessian).block(static_cast<Eigen::Index>(startRow), static_cast<unsigned int>(startCol), block.rows(), block.cols()) = toEigen(block);
+                    m_hessianBlocksSet(startRow, startCol) = true;
+                }
+            }
+
+            void setHessianBlockAndItsTranspose(MatrixDynSize& hessian, const MatrixDynSize& block, size_t startRow, size_t startCol) {
+                setHessianBlock(hessian, block, startRow, startCol);
+
+                if (m_hessianBlocksSet(startCol, startRow)) {
+                    toEigen(hessian).block(static_cast<Eigen::Index>(startCol), static_cast<unsigned int>(startRow), block.cols(), block.rows()) += toEigen(block).transpose();
+                } else {
+                    toEigen(hessian).block(static_cast<Eigen::Index>(startCol), static_cast<unsigned int>(startRow), block.cols(), block.rows()) = toEigen(block).transpose();
+                    m_hessianBlocksSet(startCol, startRow) = true;
+                }
+
+
             }
 
             void mergeSparsityVectors(const std::vector<SparsityStructure>& original, SparsityStructure& merged) {
@@ -335,6 +410,23 @@ namespace iDynTree {
                 if ((m_constraintsControlJacBuffer.rows() != m_constraintsPerInstant) || (m_constraintsControlJacBuffer.cols() != m_nu)) {
                     m_constraintsStateJacBuffer.resize(static_cast<unsigned int>(m_constraintsPerInstant), static_cast<unsigned int>(m_nu));
                 }
+
+                m_lambdaCollocation.resize(static_cast<unsigned int>(m_nx));
+                m_lambdaConstraints.resize(static_cast<unsigned int>(m_constraintsPerInstant));
+                m_stateHessianBuffer.resize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nx));
+                m_controlHessianBuffer.resize(static_cast<unsigned int>(m_nu), static_cast<unsigned int>(m_nu));
+                m_stateControlHessianBuffer.resize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
+                m_stateCollocationHessians[CollocationHessianIndex(0,0)] = m_stateHessianBuffer;
+                m_stateCollocationHessians[CollocationHessianIndex(0,1)] = m_stateHessianBuffer;
+                m_stateCollocationHessians[CollocationHessianIndex(1,1)] = m_stateHessianBuffer;
+                m_controlCollocationHessians[CollocationHessianIndex(0,0)] = m_controlHessianBuffer;
+                m_controlCollocationHessians[CollocationHessianIndex(0,1)] = m_controlHessianBuffer;
+                m_controlCollocationHessians[CollocationHessianIndex(1,1)] = m_controlHessianBuffer;
+                m_stateControCollocationlHessians[CollocationHessianIndex(0,0)] = MatrixDynSize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
+                m_stateControCollocationlHessians[CollocationHessianIndex(0,1)] = MatrixDynSize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
+                m_stateControCollocationlHessians[CollocationHessianIndex(1,0)] = MatrixDynSize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
+                m_stateControCollocationlHessians[CollocationHessianIndex(1,1)] = MatrixDynSize(static_cast<unsigned int>(m_nx), static_cast<unsigned int>(m_nu));
+
             }
 
 
@@ -1001,7 +1093,7 @@ namespace iDynTree {
                 m_infoData->costIsNonLinear = !(m_ocproblem->hasOnlyLinearCosts()) && !(m_ocproblem->hasOnlyQuadraticCosts());
                 m_infoData->hasSparseHessian = true;
                 m_infoData->hasSparseConstraintJacobian = true;
-                m_infoData->hessianIsProvided = !(m_infoData->hasNonLinearConstraints);
+                m_infoData->hessianIsProvided = true;
 
                 allocateBuffers();
 
@@ -1026,6 +1118,7 @@ namespace iDynTree {
                 }
 
                 resetNonZerosCount();
+                m_hessianBlocksSet.clear();
 
                 std::vector<MeshPoint>::iterator mesh = m_meshPoints.begin(), previousControlMesh = mesh;
                 size_t index = 0, constraintIndex = 0;
@@ -1072,16 +1165,11 @@ namespace iDynTree {
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        //Saving the hessian structure
-                        if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
+                        if (!m_info.costIsLinear() || m_info.hasNonLinearConstraints()) {
                             addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
                             addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
                             addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                            addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
-
-                            addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
-                            addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
                         }
 
                     } else if (mesh->type == MeshPointType::Control) {
@@ -1152,20 +1240,29 @@ namespace iDynTree {
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
+                        if (!m_info.costIsLinear() || m_info.hasNonLinearConstraints()) {
                             addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+
                             addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
                             addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                            addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
 
-                            addHessianBlock(mesh->previousControlIndex, nu, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and u-1
-                            addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
                         }
 
-                        if (!(m_ocproblem->systemIsLinear())) {
-                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                        if (!(m_ocproblem->systemIsLinear())) { //if the system is not linear, automatically the above if is true
+
+                            addHessianBlock(mesh->previousControlIndex, nu, mesh->previousControlIndex, nu);
+                            addHessianBlock((mesh-1)->stateIndex, nx, (mesh-1)->stateIndex, nx);
+
                             addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+
+                            addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
+
+                            addHessianBlock(mesh->controlIndex, nu, mesh->previousControlIndex, nu);
+
+                            addHessianBlock(mesh->controlIndex, nu, (mesh - 1)->stateIndex, nx);
+
+                            addHessianBlock(mesh->previousControlIndex, nu, (mesh - 1)->stateIndex, nx);
                         }
 
 
@@ -1231,17 +1328,24 @@ namespace iDynTree {
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        if (!m_info.costIsLinear() && m_info.hasLinearConstraints()) {
+                        if (!m_info.costIsLinear() || m_info.hasNonLinearConstraints()) {
                             addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
+
                             addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
 
                             addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-                            addHessianBlock(mesh->stateIndex, nx, mesh->controlIndex, nu);
+
                         }
 
-                        if (!(m_ocproblem->systemIsLinear())) {
-                            addHessianBlock((mesh - 1)->stateIndex, nx, mesh->stateIndex, nx); //assume that due to the dynamics we have a cross relation between x and x-1
+                        if (!(m_ocproblem->systemIsLinear())) { //if the system is not linear, automatically the above if is true
+
+                            addHessianBlock((mesh-1)->stateIndex, nx, (mesh-1)->stateIndex, nx);
+
                             addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
+
+                            addHessianBlock(mesh->controlIndex, nu, (mesh - 1)->stateIndex, nx);
+
+                            //some blocks are missing since the previous control index and controlIndex coincide
                         }
 
                     }
@@ -1857,13 +1961,139 @@ namespace iDynTree {
             }
 
             virtual bool evaluateConstraintsHessian(const VectorDynSize& constraintsMultipliers, MatrixDynSize& hessian) override {
-                if (m_info.hasNonLinearConstraints()) {
-                    if (!(toEigen(constraintsMultipliers).isZero(0))){
-                        reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", "The constraints hessian is currently unavailable.");
-                        return false;
+
+                if (!(m_prepared)){
+                    reportError("MultipleShootingTranscription", "evaluateConstraints", "First you need to call the prepare method");
+                    return false;
+                }
+
+                if (!m_info.hasNonLinearConstraints()) {
+                    hessian.resize(static_cast<unsigned int>(m_numberOfVariables), static_cast<unsigned int>(m_numberOfVariables));
+                    hessian.zero();
+                    return true;
+                }
+
+                Eigen::Map<Eigen::VectorXd> variablesBuffer = toEigen(m_variablesBuffer);
+                Eigen::Map<Eigen::VectorXd> currentState = toEigen(m_collocationStateBuffer[1]);
+                Eigen::Map<Eigen::VectorXd> previousState = toEigen(m_collocationStateBuffer[0]);
+                Eigen::Map<Eigen::VectorXd> currentControl = toEigen(m_collocationControlBuffer[1]);
+                Eigen::Map<Eigen::VectorXd> previousControl = toEigen(m_collocationControlBuffer[0]);
+                Eigen::Map<Eigen::VectorXd> lambdaCollocation = toEigen(m_lambdaCollocation);
+                Eigen::Map<Eigen::VectorXd> lambdaConstraints = toEigen(m_lambdaConstraints);
+                Eigen::Map<const Eigen::VectorXd> fullLambda = toEigen(constraintsMultipliers);
+
+
+                Eigen::Index nx = static_cast<Eigen::Index>(m_nx);
+                Eigen::Index nu = static_cast<Eigen::Index>(m_nu);
+                Eigen::Index nc = static_cast<Eigen::Index>(m_constraintsPerInstant);
+
+                if ((hessian.rows() != numberOfVariables()) || (hessian.cols() != numberOfVariables())) {
+                    hessian.resize(static_cast<unsigned int>(numberOfVariables()), static_cast<unsigned int>(numberOfVariables()));
+                }
+
+                m_hessianBlocksSet.reset();
+
+                MeshPointOrigin first = MeshPointOrigin::FirstPoint();
+                Eigen::Index constraintIndex = 0;
+                double dT = 0;
+                for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
+
+                    currentState = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
+                    currentControl  = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->controlIndex), nu);
+                    previousControl = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->previousControlIndex), nu);
+
+                    if (mesh->origin != first) {
+                        previousState = variablesBuffer.segment(static_cast<Eigen::Index>((mesh - 1)->stateIndex), nx);
+                        lambdaCollocation = fullLambda.segment(constraintIndex, nx);
+                        dT = mesh->time - (mesh - 1)->time;
+
+                        if (!(m_integrator->evaluateCollocationConstraintSecondDerivatives(mesh->time, m_collocationStateBuffer, m_collocationControlBuffer, dT,
+                                                                                           m_lambdaCollocation, m_stateCollocationHessians,
+                                                                                           m_controlCollocationHessians, m_stateControCollocationlHessians))){
+                            std::ostringstream errorMsg;
+                            errorMsg << "Error while evaluating the collocation constraint hessian at time " << mesh->time << ".";
+                            reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
+                            return false;
+                        }
+
+                        setHessianBlock(hessian, m_stateCollocationHessians[CollocationHessianIndex(0, 0)], (mesh-1)->stateIndex, (mesh-1)->stateIndex);
+                        setHessianBlockAndItsTranspose(hessian, m_stateCollocationHessians[CollocationHessianIndex(0, 1)], (mesh-1)->stateIndex, mesh->stateIndex);
+                        setHessianBlock(hessian, m_stateCollocationHessians[CollocationHessianIndex(1, 1)], mesh->stateIndex, mesh->stateIndex);
+
+                        setHessianBlock(hessian, m_controlCollocationHessians[CollocationHessianIndex(0,0)], mesh->previousControlIndex, mesh->previousControlIndex);
+                        setHessianBlockAndItsTranspose(hessian, m_controlCollocationHessians[CollocationHessianIndex(0,1)], mesh->previousControlIndex, mesh->controlIndex);
+                        setHessianBlock(hessian, m_controlCollocationHessians[CollocationHessianIndex(1,1)], mesh->controlIndex, mesh->controlIndex);
+
+                        setHessianBlockAndItsTranspose(hessian, m_stateControCollocationlHessians[CollocationHessianIndex(0,0)], (mesh-1)->stateIndex, mesh->previousControlIndex);
+                        setHessianBlockAndItsTranspose(hessian, m_stateControCollocationlHessians[CollocationHessianIndex(0,1)], (mesh-1)->stateIndex, mesh->controlIndex);
+                        setHessianBlockAndItsTranspose(hessian, m_stateControCollocationlHessians[CollocationHessianIndex(1,0)], mesh->stateIndex, mesh->previousControlIndex);
+                        setHessianBlockAndItsTranspose(hessian, m_stateControCollocationlHessians[CollocationHessianIndex(1,1)], mesh->stateIndex, mesh->controlIndex);
+
+                        constraintIndex += nx;
+                    }
+
+
+                    if (nc != 0) {
+                        if (mesh->origin == first) {
+
+                            lambdaConstraints = fullLambda.segment(constraintIndex, nc);
+
+                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTStateControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_lambdaConstraints, m_stateControlHessianBuffer))){
+                                std::ostringstream errorMsg;
+                                errorMsg << "Error while evaluating the constraints hessian wrt state and control at time " << mesh->time << ".";
+                                reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
+                                return false;
+                            }
+
+                            setHessianBlockAndItsTranspose(hessian, m_stateControlHessianBuffer, mesh->stateIndex, mesh->controlIndex);
+
+                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
+                                std::ostringstream errorMsg;
+                                errorMsg << "Error while evaluating the constraints hessian wrt control at time " << mesh->time << ".";
+                                reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
+                                return false;
+                            }
+
+                            setHessianBlock(hessian, m_controlHessianBuffer, mesh->controlIndex, mesh->controlIndex);
+
+                        } else {
+
+                            lambdaConstraints = fullLambda.segment(constraintIndex, nc);
+
+                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTState(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_stateHessianBuffer))){
+                                std::ostringstream errorMsg;
+                                errorMsg << "Error while evaluating the constraints hessian wrt state at time " << mesh->time << ".";
+                                reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
+                                return false;
+                            }
+
+                            setHessianBlock(hessian, m_stateHessianBuffer, mesh->stateIndex, mesh->stateIndex);
+
+                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTStateControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_stateControlHessianBuffer))){
+                                std::ostringstream errorMsg;
+                                errorMsg << "Error while evaluating the constraints hessian wrt state and control at time " << mesh->time << ".";
+                                reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
+                                return false;
+                            }
+
+                            setHessianBlockAndItsTranspose(hessian, m_stateControlHessianBuffer, mesh->stateIndex, mesh->controlIndex);
+
+                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
+                                std::ostringstream errorMsg;
+                                errorMsg << "Error while evaluating the constraints hessian wrt control at time " << mesh->time << ".";
+                                reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
+                                return false;
+                            }
+
+                            setHessianBlock(hessian, m_controlHessianBuffer, mesh->controlIndex, mesh->controlIndex);
+
+                        }
+                        constraintIndex += nc;
                     }
+
                 }
-                hessian.zero();
+                assert(static_cast<size_t>(constraintIndex) == m_numberOfConstraints);
+
                 return true;
             }
         };
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 072d9307a92..35ac88ddbb8 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -45,6 +45,7 @@ namespace iDynTree {
             std::shared_ptr<ConstraintsGroup> group_ptr;
             VectorDynSize constraintsBuffer;
             MatrixDynSize stateJacobianBuffer, controlJacobianBuffer;
+            VectorDynSize lambdaBuffer;
             SparsityStructure stateSparsity;
             SparsityStructure controlSparsity;
             bool hasStateSparsity = false;
@@ -74,6 +75,7 @@ namespace iDynTree {
             CostsMap costs;
             VectorDynSize costStateJacobianBuffer, costControlJacobianBuffer;
             MatrixDynSize costStateHessianBuffer, costControlHessianBuffer, costMixedHessianBuffer;
+            MatrixDynSize constraintsStateHessianBuffer, constraintsControlHessianBuffer, constraintsMixedHessianBuffer;
             bool stateLowerBounded, stateUpperBounded, controlLowerBounded, controlUpperBounded;
             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
             SparsityStructure stateSparsity;
@@ -212,6 +214,7 @@ namespace iDynTree {
                 newGroup->stateJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize()));
                 newGroup->controlJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize()));
             }
+            newGroup->lambdaBuffer.resize(groupOfConstraints->constraintsDimension());
 
             newGroup->hasStateSparsity = groupOfConstraints->constraintJacobianWRTStateSparsity(newGroup->stateSparsity); //needed to allocate memory in advance
             newGroup->hasControlSparsity = groupOfConstraints->constraintJacobianWRTControlSparsity(newGroup->controlSparsity); //needed to allocate memory in advance
@@ -1313,5 +1316,118 @@ namespace iDynTree {
             return true;
         }
 
+        bool OptimalControlProblem::constraintSecondPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        {
+            if ((hessian.rows() != state.size()) || (hessian.cols() != state.size())) {
+                hessian.resize(state.size(), state.size());
+            }
+
+            if ((m_pimpl->constraintsStateHessianBuffer.rows() != state.size()) || (m_pimpl->constraintsStateHessianBuffer.cols() != state.size())) {
+                m_pimpl->constraintsStateHessianBuffer.resize(state.size(), state.size());
+            }
+
+            if (m_pimpl->constraintsGroups.size() == 0) {
+                hessian.zero();
+                return true;
+            }
+
+            bool first = true;
+            Eigen::Index offset = 0;
+
+            for (auto& group : m_pimpl->constraintsGroups){
+                toEigen(group.second->lambdaBuffer) = toEigen(lambda).segment(offset, group.second->group_ptr->constraintsDimension());
+                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTState(time, state, control, group.second->lambdaBuffer, m_pimpl->constraintsStateHessianBuffer)){
+                    std::ostringstream errorMsg;
+                    errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
+                    reportError("OptimalControlProblem", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
+                    return false;
+                }
+                if (first){
+                    toEigen(hessian) = toEigen(m_pimpl->constraintsStateHessianBuffer);
+                    first = false;
+                } else {
+                    toEigen(hessian) += toEigen(m_pimpl->constraintsStateHessianBuffer);
+                }
+                offset += group.second->group_ptr->constraintsDimension();
+            }
+
+            return true;
+        }
+
+        bool OptimalControlProblem::constraintSecondPartialDerivativeWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        {
+            if ((hessian.rows() != control.size()) || (hessian.cols() != control.size())) {
+                hessian.resize(control.size(), control.size());
+            }
+
+            if ((m_pimpl->constraintsControlHessianBuffer.rows() != control.size()) || (m_pimpl->constraintsControlHessianBuffer.cols() != control.size())) {
+                m_pimpl->constraintsControlHessianBuffer.resize(control.size(), control.size());
+            }
+
+            if (m_pimpl->constraintsGroups.size() == 0) {
+                hessian.zero();
+                return true;
+            }
+
+            bool first = true;
+            Eigen::Index offset = 0;
+
+            for (auto& group : m_pimpl->constraintsGroups){
+                toEigen(group.second->lambdaBuffer) = toEigen(lambda).segment(offset, group.second->group_ptr->constraintsDimension());
+                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTControl(time, state, control, group.second->lambdaBuffer, m_pimpl->constraintsControlHessianBuffer)){
+                    std::ostringstream errorMsg;
+                    errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
+                    reportError("OptimalControlProblem", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
+                    return false;
+                }
+                if (first){
+                    toEigen(hessian) = toEigen(m_pimpl->constraintsControlHessianBuffer);
+                    first = false;
+                } else {
+                    toEigen(hessian) += toEigen(m_pimpl->constraintsControlHessianBuffer);
+                }
+                offset += group.second->group_ptr->constraintsDimension();
+            }
+            return true;
+        }
+
+        bool OptimalControlProblem::constraintSecondPartialDerivativeWRTStateControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        {
+            if ((hessian.rows() != state.size()) || (hessian.cols() != control.size())) {
+                hessian.resize(state.size(), control.size());
+            }
+
+            if ((m_pimpl->constraintsMixedHessianBuffer.rows() != state.size()) || (m_pimpl->constraintsMixedHessianBuffer.cols() != control.size())) {
+                m_pimpl->constraintsMixedHessianBuffer.resize(state.size(), control.size());
+            }
+
+            if (m_pimpl->constraintsGroups.size() == 0) {
+                hessian.zero();
+                return true;
+            }
+
+            bool first = true;
+            Eigen::Index offset = 0;
+
+            for (auto& group : m_pimpl->constraintsGroups){
+                toEigen(group.second->lambdaBuffer) = toEigen(lambda).segment(offset, group.second->group_ptr->constraintsDimension());
+
+                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTStateControl(time, state, control, group.second->lambdaBuffer, m_pimpl->constraintsMixedHessianBuffer)){
+                    std::ostringstream errorMsg;
+                    errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
+                    reportError("OptimalControlProblem", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
+                    return false;
+                }
+                if (first){
+                    toEigen(hessian) = toEigen(m_pimpl->constraintsMixedHessianBuffer);
+                    first = false;
+                } else {
+                    toEigen(hessian) += toEigen(m_pimpl->constraintsMixedHessianBuffer);
+                }
+                offset += group.second->group_ptr->constraintsDimension();
+            }
+            return true;
+        }
+
     }
 }

From 2cc1271f22b7e3852be854fdff9a655bd1ffca88 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 6 Feb 2019 19:11:12 +0100
Subject: [PATCH 104/194] Added possibility to use approximated hessians ins
 optimizers were supported.

---
 .../include/iDynTree/Optimizers/IpoptInterface.h         | 2 ++
 .../include/iDynTree/Optimizers/WorhpInterface.h         | 2 ++
 src/optimalcontrol/src/IpoptInterface.cpp                | 9 ++++++++-
 src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp  | 5 +++++
 src/optimalcontrol/src/MultipleShootingSolver.cpp        | 7 ++++---
 src/optimalcontrol/src/WorhpInterface.cpp                | 9 ++++++++-
 src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp  | 5 +++++
 src/optimalcontrol/tests/OptimalControlIpoptTest.cpp     | 2 ++
 8 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
index 5aadb8a4a81..d98f67240e8 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/IpoptInterface.h
@@ -65,6 +65,8 @@ namespace iDynTree {
 
             virtual double plusInfinity() override;
 
+            void useApproximatedHessians(bool useApproximatedHessian = true);
+
             bool setIpoptOption(const std::string &tag, const std::string &value);
 
             bool setIpoptOption(const std::string &tag, double value);
diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
index 059c2fa68b3..e7272e5e6fe 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
@@ -63,6 +63,8 @@ namespace iDynTree {
 
             virtual double plusInfinity() override;
 
+            void useApproximatedHessians(bool useApproximatedHessian = true);
+
             bool setWorhpParam(const std::string &paramName, bool value);
 
             bool setWorhpParam(const std::string &paramName, double value);
diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index c24d1d28a42..b6ccc038c03 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -462,6 +462,7 @@ namespace iDynTree {
             Ipopt::SmartPtr<Ipopt::IpoptApplication> loader;
             unsigned int previousNumberOfVariables, previousNumberOfConstraints;
             size_t previousJacobianNonZeros, previousHessianNonZeros;
+            bool useApproximatedHessian;
 
             IpoptInterfaceImplementation()
             : nlpPointer(new NLPImplementation())
@@ -470,6 +471,7 @@ namespace iDynTree {
             , previousNumberOfConstraints(0)
             , previousJacobianNonZeros(0)
             , previousHessianNonZeros(0)
+            , useApproximatedHessian(false)
             {
             }
 
@@ -532,7 +534,7 @@ namespace iDynTree {
 
             m_pimpl->nlpPointer->numberOfConstraints = m_problem->numberOfConstraints();
 
-            if (!(m_problem->info().hessianIsProvided())) {
+            if (!(m_problem->info().hessianIsProvided()) || (m_pimpl->useApproximatedHessian)) {
                 m_pimpl->loader->Options()->SetStringValue("hessian_approximation", "limited-memory");
             }
 
@@ -696,6 +698,11 @@ namespace iDynTree {
             return static_cast<double>(output);
         }
 
+        void IpoptInterface::useApproximatedHessians(bool useApproximatedHessian)
+        {
+            m_pimpl->useApproximatedHessian = useApproximatedHessian;
+        }
+
         bool IpoptInterface::setIpoptOption(const std::string &tag, const std::string &value)
         {
             return m_pimpl->loader->Options()->SetStringValue(tag, value);
diff --git a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
index aebe7431aff..887f52b1233 100644
--- a/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/IpoptInterfaceNotImplemented.cpp
@@ -83,6 +83,11 @@ double IpoptInterface::plusInfinity()
     return Optimizer::plusInfinity();
 }
 
+void IpoptInterface::useApproximatedHessians(bool useApproximatedHessian)
+{
+    reportError("IpoptInterface", "useApproximatedHessians", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
+}
+
 bool IpoptInterface::setIpoptOption(const std::string &tag, const std::string &value)
 {
     reportError("IpoptInterface", "setIpoptOption", "IpoptInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_IPOPT set to ON?");
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index d8d7984637c..a4b9c7479aa 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -95,12 +95,13 @@ namespace iDynTree {
 
             bool& operator()(size_t row, size_t col) {
                 CollocationHessianIndex indices(row, col);
-                auto it = m_map.find(indices);
+                std::map<CollocationHessianIndex, bool>::iterator it = m_map.find(indices);
 
                 if (it == m_map.end()) {
-                    m_map[indices] = false;
+                    std::pair<std::map<CollocationHessianIndex, bool>::iterator, bool> result = m_map.insert(std::make_pair(indices, false));
+                    it = result.first;
                 }
-                return m_map[indices];
+                return it->second;
             }
 
             void clear() {
diff --git a/src/optimalcontrol/src/WorhpInterface.cpp b/src/optimalcontrol/src/WorhpInterface.cpp
index f3bc657bf9c..cd742cbd9e5 100644
--- a/src/optimalcontrol/src/WorhpInterface.cpp
+++ b/src/optimalcontrol/src/WorhpInterface.cpp
@@ -71,6 +71,7 @@ class WorhpInterface::WorhpInterfaceImplementation {
     bool sparseJacobian, sparseHessian;
     bool previouslySolved;
     bool firstTime;
+    bool useApproximatedHessian;
 
 
     void resizeBuffers(unsigned int numberOfVariables, unsigned int numberOfConstraints) {
@@ -120,6 +121,7 @@ WorhpInterface::WorhpInterface()
     CHECK_WORHP_VERSION;
     m_pimpl->previouslySolved = false;
     m_pimpl->firstTime = true;
+    m_pimpl->useApproximatedHessian = false;
     /*
     * Parameter initialisation routine
     */
@@ -202,7 +204,7 @@ bool WorhpInterface::solve()
         m_pimpl->sparseJacobian = false;
     }
 
-    if (!(m_problem->info().hessianIsProvided())) {
+    if (!(m_problem->info().hessianIsProvided()) || (m_pimpl->useApproximatedHessian)) {
         if (!setWorhpParam("UserHM", false)) {
             reportError("WorhpInterface", "solve", "Failed to set UserHM option.");
             m_pimpl->previouslySolved = false;
@@ -776,6 +778,11 @@ double WorhpInterface::plusInfinity()
     return m_pimpl->worhp.par.Infty;
 }
 
+void WorhpInterface::useApproximatedHessians(bool useApproximatedHessian)
+{
+    m_pimpl->useApproximatedHessian = useApproximatedHessian;
+}
+
 bool WorhpInterface::setWorhpParam(const std::string &paramName, bool value)
 {
     if (!WorhpSetBoolParam(&(m_pimpl->worhp.par) ,paramName.c_str() , value)) {
diff --git a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
index 2f5db60a2a5..f361febb3ca 100644
--- a/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
+++ b/src/optimalcontrol/src/WorhpInterfaceNotImplemented.cpp
@@ -84,6 +84,11 @@ double WorhpInterface::plusInfinity()
     return Optimizer::plusInfinity();
 }
 
+void WorhpInterface::useApproximatedHessians(bool useApproximatedHessian)
+{
+    reportError("WorhpInterface", "useApproximatedHessians", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
+}
+
 bool WorhpInterface::setWorhpParam(const std::string &paramName, bool value)
 {
     reportError("WorhpInterface", "setWorhpParam", "WorhpInterface not implemented. Have you compiled iDynTree with the IDYNTREE_USES_WORHP set to ON?");
diff --git a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
index f0a247fb04d..abedd918602 100644
--- a/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
+++ b/src/optimalcontrol/tests/OptimalControlIpoptTest.cpp
@@ -258,6 +258,8 @@ int main () {
     //ASSERT_IS_TRUE(optimizer->setIpoptOption("linear_solver", "ma27"));
     ASSERT_IS_TRUE(optimizer->setIpoptOption("print_level", 0));
 
+    optimizer->useApproximatedHessians();
+
     clock_t initT, endT;
     initT = clock();
     ASSERT_IS_TRUE(solver.solve());

From 9280e1c5f4844743c12dd0b13af5ce0fa8143681 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Feb 2019 09:35:49 +0100
Subject: [PATCH 105/194] Setting hessians in linear objects.

Updated test on MultipleShooting.
---
 .../include/iDynTree/DynamicalSystem.h        |   6 +-
 .../include/iDynTree/LinearConstraint.h       |  19 ++++
 .../include/iDynTree/LinearSystem.h           |  18 +++
 src/optimalcontrol/src/ConstraintsGroup.cpp   |   2 +-
 src/optimalcontrol/src/LinearConstraint.cpp   |  21 ++++
 src/optimalcontrol/src/LinearSystem.cpp       |  21 ++++
 .../tests/MultipleShootingTest.cpp            | 104 ++++++++++++++----
 7 files changed, 166 insertions(+), 25 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index 4463a671316..fe87b2f6899 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -199,7 +199,7 @@ namespace optimalcontrol {
         virtual bool dynamicsSecondPartialDerivativeWRTState(double time,
                                                              const iDynTree::VectorDynSize& state,
                                                              const iDynTree::VectorDynSize& control,
-                                                             const VectorDynSize& lambda,
+                                                             const iDynTree::VectorDynSize& lambda,
                                                              iDynTree::MatrixDynSize& partialDerivative);
 
         /**
@@ -216,7 +216,7 @@ namespace optimalcontrol {
         virtual bool dynamicsSecondPartialDerivativeWRTControl(double time,
                                                                const iDynTree::VectorDynSize& state,
                                                                const iDynTree::VectorDynSize& control,
-                                                               const VectorDynSize& lambda,
+                                                               const iDynTree::VectorDynSize& lambda,
                                                                iDynTree::MatrixDynSize& partialDerivative);
 
         /**
@@ -234,7 +234,7 @@ namespace optimalcontrol {
         virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double time,
                                                                     const iDynTree::VectorDynSize& state,
                                                                     const iDynTree::VectorDynSize& control,
-                                                                    const VectorDynSize& lambda,
+                                                                    const iDynTree::VectorDynSize& lambda,
                                                                     iDynTree::MatrixDynSize& partialDerivative);
 
     private:
diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
index d780bc73bd5..07d4a9b8712 100644
--- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h
+++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
@@ -78,6 +78,25 @@ namespace iDynTree {
 
             virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
 
+            virtual bool constraintSecondPartialDerivativeWRTState(double time,
+                                                                   const VectorDynSize& state,
+                                                                   const VectorDynSize& control,
+                                                                   const VectorDynSize& lambda,
+                                                                   MatrixDynSize& hessian) final;
+
+            virtual bool constraintSecondPartialDerivativeWRTControl(double time,
+                                                                     const VectorDynSize& state,
+                                                                     const VectorDynSize& control,
+                                                                     const VectorDynSize& lambda,
+                                                                     MatrixDynSize& hessian) final;
+
+
+            virtual bool constraintSecondPartialDerivativeWRTStateControl(double time,
+                                                                          const VectorDynSize& state,
+                                                                          const VectorDynSize& control,
+                                                                          const VectorDynSize& lambda,
+                                                                          MatrixDynSize& hessian) final;
+
         private:
             class LinearConstraintImplementation;
             LinearConstraintImplementation* m_pimpl;
diff --git a/src/optimalcontrol/include/iDynTree/LinearSystem.h b/src/optimalcontrol/include/iDynTree/LinearSystem.h
index 6ef672bb8a3..8136720b32b 100644
--- a/src/optimalcontrol/include/iDynTree/LinearSystem.h
+++ b/src/optimalcontrol/include/iDynTree/LinearSystem.h
@@ -74,6 +74,24 @@ namespace iDynTree {
 
             virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
 
+            virtual bool dynamicsSecondPartialDerivativeWRTState(double time,
+                                                                 const iDynTree::VectorDynSize& state,
+                                                                 const iDynTree::VectorDynSize& control,
+                                                                 const iDynTree::VectorDynSize& lambda,
+                                                                 iDynTree::MatrixDynSize& partialDerivative) final;
+
+            virtual bool dynamicsSecondPartialDerivativeWRTControl(double time,
+                                                                   const iDynTree::VectorDynSize& state,
+                                                                   const iDynTree::VectorDynSize& control,
+                                                                   const iDynTree::VectorDynSize& lambda,
+                                                                   iDynTree::MatrixDynSize& partialDerivative) final;
+
+            virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double time,
+                                                                        const iDynTree::VectorDynSize& state,
+                                                                        const iDynTree::VectorDynSize& control,
+                                                                        const iDynTree::VectorDynSize& lambda,
+                                                                        iDynTree::MatrixDynSize& partialDerivative) final;
+
 
         private:
             class LinearSystemPimpl;
diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index efb68833d50..ca2a1298041 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -723,7 +723,7 @@ namespace optimalcontrol {
                 return true;
             }
 
-            hessian.resize(state.size(), state.size());
+            hessian.resize(state.size(), control.size());
 
             std::vector< TimedConstraint_ptr >::reverse_iterator constraintIterator = m_pimpl->findActiveConstraint(time);
             if (constraintIterator == m_pimpl->orderedIntervals.rend()){ //no active constraint
diff --git a/src/optimalcontrol/src/LinearConstraint.cpp b/src/optimalcontrol/src/LinearConstraint.cpp
index 948bc36110b..1c7b05307d1 100644
--- a/src/optimalcontrol/src/LinearConstraint.cpp
+++ b/src/optimalcontrol/src/LinearConstraint.cpp
@@ -256,5 +256,26 @@ namespace iDynTree {
             return true;
         }
 
+        bool LinearConstraint::constraintSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &state, const VectorDynSize &/*control*/, const VectorDynSize &/*lambda*/, MatrixDynSize &hessian)
+        {
+            hessian.resize(state.size(), state.size());
+            hessian.zero();
+            return true;
+        }
+
+        bool LinearConstraint::constraintSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &control, const VectorDynSize &/*lambda*/, MatrixDynSize &hessian)
+        {
+            hessian.resize(control.size(), control.size());
+            hessian.zero();
+            return true;
+        }
+
+        bool LinearConstraint::constraintSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &/*lambda*/, MatrixDynSize &hessian)
+        {
+            hessian.resize(state.size(), control.size());
+            hessian.zero();
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp
index ea613dd3dd2..1935c5a6c65 100644
--- a/src/optimalcontrol/src/LinearSystem.cpp
+++ b/src/optimalcontrol/src/LinearSystem.cpp
@@ -243,5 +243,26 @@ namespace iDynTree {
             return true;
         }
 
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &state, const VectorDynSize &/*control*/, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
+        {
+            partialDerivative.resize(state.size(), state.size());
+            partialDerivative.zero();
+            return true;
+        }
+
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &control, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
+        {
+            partialDerivative.resize(control.size(), control.size());
+            partialDerivative.zero();
+            return true;
+        }
+
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
+        {
+            partialDerivative.resize(state.size(), control.size());
+            partialDerivative.zero();
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index e49d1e28d37..30db48e501e 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -37,7 +37,7 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
     TestSystem() : iDynTree::optimalcontrol::DynamicalSystem(2,3) {}
     ~TestSystem() override;
 
-    virtual bool dynamics(const iDynTree::VectorDynSize &state, double time, iDynTree::VectorDynSize &stateDynamics) override {
+    virtual bool dynamics(const iDynTree::VectorDynSize &state, double /*time*/, iDynTree::VectorDynSize &stateDynamics) override {
         if (state.size() != 2)
             return false;
 
@@ -49,7 +49,7 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
     }
 
     virtual bool dynamicsStateFirstDerivative(const iDynTree::VectorDynSize& state,
-                                              double time,
+                                              double /*time*/,
                                               iDynTree::MatrixDynSize& dynamicsDerivative) override {
         if (state.size() != 2)
             return false;
@@ -63,7 +63,7 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
     }
 
     virtual bool dynamicsControlFirstDerivative(const iDynTree::VectorDynSize& state,
-                                                double time,
+                                                double /*time*/,
                                                 iDynTree::MatrixDynSize& dynamicsDerivative) override {
         if (state.size() != 2)
             return false;
@@ -94,6 +94,36 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
         controlSparsity = sparsity;
         return true;
     }
+
+    virtual bool dynamicsSecondPartialDerivativeWRTState(double /*time*/,
+                                                         const iDynTree::VectorDynSize& state,
+                                                         const iDynTree::VectorDynSize& /*control*/,
+                                                         const iDynTree::VectorDynSize& /*lambda*/,
+                                                         iDynTree::MatrixDynSize& partialDerivative) override {
+        partialDerivative.resize(state.size(), state.size());
+        partialDerivative.zero();
+        return true;
+    }
+
+    virtual bool dynamicsSecondPartialDerivativeWRTControl(double /*time*/,
+                                                           const iDynTree::VectorDynSize& /*state*/,
+                                                           const iDynTree::VectorDynSize& control,
+                                                           const iDynTree::VectorDynSize& /*lambda*/,
+                                                           iDynTree::MatrixDynSize& partialDerivative) override {
+        partialDerivative.resize(control.size(), control.size());
+        partialDerivative.zero();
+        return true;
+    }
+
+    virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/,
+                                                                const iDynTree::VectorDynSize& state,
+                                                                const iDynTree::VectorDynSize& control,
+                                                                const iDynTree::VectorDynSize& /*lambda*/,
+                                                                iDynTree::MatrixDynSize& partialDerivative) override {
+        partialDerivative.resize(state.size(), control.size());
+        partialDerivative.zero();
+        return true;
+    }
 };
 TestSystem::~TestSystem(){};
 
@@ -115,7 +145,7 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
     }
     virtual ~TestConstraint() override;
 
-    virtual bool evaluateConstraint(double time,
+    virtual bool evaluateConstraint(double /*time*/,
                                     const iDynTree::VectorDynSize& state,
                                     const iDynTree::VectorDynSize& control,
                                     iDynTree::VectorDynSize& constraint) override {
@@ -132,18 +162,18 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
         return true;
     }
 
-    virtual bool constraintJacobianWRTState(double time,
-                                            const iDynTree::VectorDynSize& state,
-                                            const iDynTree::VectorDynSize& control,
+    virtual bool constraintJacobianWRTState(double /*time*/,
+                                            const iDynTree::VectorDynSize& /*state*/,
+                                            const iDynTree::VectorDynSize& /*control*/,
                                             iDynTree::MatrixDynSize& jacobian) override {
         ASSERT_IS_TRUE((jacobian.rows() == 1) && (jacobian.cols() == 2));
         jacobian.zero();
         return true;
     }
 
-    virtual bool constraintJacobianWRTControl(double time,
-                                              const iDynTree::VectorDynSize& state,
-                                              const iDynTree::VectorDynSize& control,
+    virtual bool constraintJacobianWRTControl(double /*time*/,
+                                              const iDynTree::VectorDynSize& /*state*/,
+                                              const iDynTree::VectorDynSize& /*control*/,
                                               iDynTree::MatrixDynSize& jacobian) override {
         ASSERT_IS_TRUE((jacobian.rows() == 1) && (jacobian.cols() == 3));
         jacobian.zero();
@@ -171,6 +201,36 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
         controlSparsity = sparsity;
         return true;
     }
+
+    virtual bool constraintSecondPartialDerivativeWRTState(double /*time*/,
+                                                           const iDynTree::VectorDynSize& state,
+                                                           const iDynTree::VectorDynSize& /*control*/,
+                                                           const iDynTree::VectorDynSize& /*lambda*/,
+                                                           iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(state.size(), state.size());
+        hessian.zero();
+        return true;
+    }
+
+    virtual bool constraintSecondPartialDerivativeWRTControl(double /*time*/,
+                                                             const iDynTree::VectorDynSize& /*state*/,
+                                                             const iDynTree::VectorDynSize& control,
+                                                             const iDynTree::VectorDynSize& /*lambda*/,
+                                                             iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(control.size(), control.size());
+        hessian.zero();
+        return true;
+    }
+
+    virtual bool constraintSecondPartialDerivativeWRTStateControl(double /*time*/,
+                                                                  const iDynTree::VectorDynSize& state,
+                                                                  const iDynTree::VectorDynSize& control,
+                                                                  const iDynTree::VectorDynSize& /*lambda*/,
+                                                                  iDynTree::MatrixDynSize& hessian) override {
+        hessian.resize(state.size(), control.size());
+        hessian.zero();
+        return true;
+    }
 };
 TestConstraint::~TestConstraint(){}
 
@@ -184,9 +244,9 @@ class TestCost : public iDynTree::optimalcontrol::Cost {
         :iDynTree::optimalcontrol::Cost(name)
     {}
 
-    virtual ~TestCost();
+    virtual ~TestCost() override;
 
-    virtual bool costEvaluation(double time,
+    virtual bool costEvaluation(double /*time*/,
                                 const iDynTree::VectorDynSize& state,
                                 const iDynTree::VectorDynSize& control,
                                 double& costValue) override {
@@ -199,7 +259,7 @@ class TestCost : public iDynTree::optimalcontrol::Cost {
         return true;
     }
 
-    virtual bool costFirstPartialDerivativeWRTState(double time,
+    virtual bool costFirstPartialDerivativeWRTState(double /*time*/,
                                                     const iDynTree::VectorDynSize& state,
                                                     const iDynTree::VectorDynSize& control,
                                                     iDynTree::VectorDynSize& partialDerivative) override {
@@ -216,7 +276,7 @@ class TestCost : public iDynTree::optimalcontrol::Cost {
         return true;
     }
 
-    virtual bool costFirstPartialDerivativeWRTControl(double time,
+    virtual bool costFirstPartialDerivativeWRTControl(double /*time*/,
                                                       const iDynTree::VectorDynSize& state,
                                                       const iDynTree::VectorDynSize& control,
                                                       iDynTree::VectorDynSize& partialDerivative) override {
@@ -234,7 +294,7 @@ class TestCost : public iDynTree::optimalcontrol::Cost {
         return true;
     }
 
-    virtual bool costSecondPartialDerivativeWRTState(double time,
+    virtual bool costSecondPartialDerivativeWRTState(double /*time*/,
                                                      const iDynTree::VectorDynSize& state,
                                                      const iDynTree::VectorDynSize& control,
                                                      iDynTree::MatrixDynSize& partialDerivative) override {
@@ -251,7 +311,7 @@ class TestCost : public iDynTree::optimalcontrol::Cost {
         return true;
     }
 
-    virtual bool costSecondPartialDerivativeWRTControl(double time,
+    virtual bool costSecondPartialDerivativeWRTControl(double /*time*/,
                                                        const iDynTree::VectorDynSize& state,
                                                        const iDynTree::VectorDynSize& control,
                                                        iDynTree::MatrixDynSize& partialDerivative) override {
@@ -269,7 +329,7 @@ class TestCost : public iDynTree::optimalcontrol::Cost {
           return true;
     }
 
-    virtual bool costSecondPartialDerivativeWRTStateControl(double time,
+    virtual bool costSecondPartialDerivativeWRTStateControl(double /*time*/,
                                                             const iDynTree::VectorDynSize& state,
                                                             const iDynTree::VectorDynSize& control,
                                                             iDynTree::MatrixDynSize& partialDerivative) override {
@@ -292,7 +352,7 @@ class OptimizerTest : public iDynTree::optimization::Optimizer {
 public:
     OptimizerTest() {}
 
-    virtual ~OptimizerTest() override {}
+    virtual ~OptimizerTest() override;
 
     virtual bool isAvailable() const override{
         return true;
@@ -328,13 +388,13 @@ class OptimizerTest : public iDynTree::optimization::Optimizer {
         dummyMatrix.zero();
 
         for (size_t i =0; i < nnzeroRows.size(); ++i){
-            jacobian(nnzeroRows[i], nnzeroCols[i]) = 0;
+            jacobian(static_cast<unsigned int>(nnzeroRows[i]), static_cast<unsigned int>(nnzeroCols[i])) = 0;
         }
 
         ASSERT_EQUAL_MATRIX_TOL(dummyMatrix, jacobian, iDynTree::DEFAULT_TOL); //check the sparsity structure
 //        std::cerr << "Cost Jacobian" << std::endl << dummyMatrix.toString() << std::endl << std::endl;
-        //not evaluating the constraint hessian for the moment
-
+        iDynTree::MatrixDynSize dummyHessian;
+        ASSERT_IS_TRUE(m_problem->evaluateConstraintsHessian(dummy1, dummyHessian));
         return true;
     }
 
@@ -367,6 +427,8 @@ class OptimizerTest : public iDynTree::optimization::Optimizer {
         return true;
     }
 };
+OptimizerTest::~OptimizerTest() {}
+
 
 
 int main(){

From 8dc90df37699e94e8a7ef5561e261c0ff0a2d12f Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Feb 2019 15:25:01 +0100
Subject: [PATCH 106/194] Zeroing hessian buffers.

---
 src/optimalcontrol/src/ForwardEuler.cpp       |  3 +
 .../src/ImplicitTrapezoidal.cpp               |  3 +
 .../src/OptimalControlProblem.cpp             | 70 ++++++++++++-------
 3 files changed, 52 insertions(+), 24 deletions(-)

diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index 97f7f1d68fa..65aed94f113 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -52,8 +52,11 @@ namespace iDynTree {
                 m_zeroNuNuBuffer.zero();
 
                 m_stateHessianBuffer.resize(nx, nx);
+                m_stateHessianBuffer.zero();
                 m_controlHessianBuffer.resize(nu, nu);
+                m_controlHessianBuffer.zero();
                 m_mixedHessianBuffer.resize(nx, nu);
+                m_mixedHessianBuffer.zero();
 
                 m_lambda.resize(nx);
 
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index 96835b67629..3f4b9d96789 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -58,8 +58,11 @@ namespace iDynTree {
                 m_zeroNuNuBuffer.zero();
 
                 m_stateHessianBuffer.resize(nx, nx);
+                m_stateHessianBuffer.zero();
                 m_controlHessianBuffer.resize(nu, nu);
+                m_controlHessianBuffer.zero();
                 m_mixedHessianBuffer.resize(nx, nu);
+                m_mixedHessianBuffer.zero();
 
                 m_lambda.resize(nx);
 
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index 35ac88ddbb8..b2da48123f0 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -176,6 +176,29 @@ namespace iDynTree {
                 return false;
             }
             m_pimpl->dynamicalSystem = dynamicalSystem;
+
+            unsigned int nx = static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize());
+            unsigned int nu = static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize());
+
+            m_pimpl->costStateHessianBuffer.resize(nx, nx);
+            m_pimpl->costStateHessianBuffer.zero();
+
+            m_pimpl->costControlHessianBuffer.resize(nu, nu);
+            m_pimpl->costControlHessianBuffer.zero();
+
+            m_pimpl->costMixedHessianBuffer.resize(nx, nu);
+            m_pimpl->costMixedHessianBuffer.zero();
+
+
+            m_pimpl->constraintsStateHessianBuffer.resize(nx, nx);
+            m_pimpl->constraintsStateHessianBuffer.zero();
+
+            m_pimpl->constraintsControlHessianBuffer.resize(nu, nu);
+            m_pimpl->constraintsControlHessianBuffer.zero();
+
+            m_pimpl->constraintsMixedHessianBuffer.resize(nx, nu);
+            m_pimpl->constraintsMixedHessianBuffer.zero();
+
             return true;
         }
 
@@ -187,6 +210,29 @@ namespace iDynTree {
             }
             m_pimpl->dynamicalSystem = linearSystem;
             m_pimpl->systemIsLinear = true;
+
+            unsigned int nx = static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize());
+            unsigned int nu = static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize());
+
+            m_pimpl->costStateHessianBuffer.resize(nx, nx);
+            m_pimpl->costStateHessianBuffer.zero();
+
+            m_pimpl->costControlHessianBuffer.resize(nu, nu);
+            m_pimpl->costControlHessianBuffer.zero();
+
+            m_pimpl->costMixedHessianBuffer.resize(nx, nu);
+            m_pimpl->costMixedHessianBuffer.zero();
+
+
+            m_pimpl->constraintsStateHessianBuffer.resize(nx, nx);
+            m_pimpl->constraintsStateHessianBuffer.zero();
+
+            m_pimpl->constraintsControlHessianBuffer.resize(nu, nu);
+            m_pimpl->constraintsControlHessianBuffer.zero();
+
+            m_pimpl->constraintsMixedHessianBuffer.resize(nx, nu);
+            m_pimpl->constraintsMixedHessianBuffer.zero();
+
             return true;
         }
 
@@ -955,10 +1001,6 @@ namespace iDynTree {
                 partialDerivative.resize(state.size(), state.size());
             }
 
-            if ((m_pimpl->costStateHessianBuffer.rows() != state.size()) || (m_pimpl->costStateHessianBuffer.cols() != state.size())) {
-                m_pimpl->costStateHessianBuffer.resize(state.size(), state.size());
-            }
-
             bool first = true;
 
             for (auto& cost : m_pimpl->costs){
@@ -993,10 +1035,6 @@ namespace iDynTree {
                 partialDerivative.resize(control.size(), control.size());
             }
 
-            if ((m_pimpl->costControlHessianBuffer.rows() != control.size()) || (m_pimpl->costControlHessianBuffer.cols() != control.size())) {
-                m_pimpl->costControlHessianBuffer.resize(control.size(), control.size());
-            }
-
             bool first = true;
 
             for (auto& cost : m_pimpl->costs){
@@ -1032,10 +1070,6 @@ namespace iDynTree {
                 partialDerivative.resize(state.size(), control.size());
             }
 
-            if ((m_pimpl->costMixedHessianBuffer.rows() != state.size()) || (m_pimpl->costMixedHessianBuffer.cols() != control.size())) {
-                m_pimpl->costMixedHessianBuffer.resize(state.size(), control.size());
-            }
-
             bool first = true;
 
             for (auto& cost : m_pimpl->costs){
@@ -1322,10 +1356,6 @@ namespace iDynTree {
                 hessian.resize(state.size(), state.size());
             }
 
-            if ((m_pimpl->constraintsStateHessianBuffer.rows() != state.size()) || (m_pimpl->constraintsStateHessianBuffer.cols() != state.size())) {
-                m_pimpl->constraintsStateHessianBuffer.resize(state.size(), state.size());
-            }
-
             if (m_pimpl->constraintsGroups.size() == 0) {
                 hessian.zero();
                 return true;
@@ -1360,10 +1390,6 @@ namespace iDynTree {
                 hessian.resize(control.size(), control.size());
             }
 
-            if ((m_pimpl->constraintsControlHessianBuffer.rows() != control.size()) || (m_pimpl->constraintsControlHessianBuffer.cols() != control.size())) {
-                m_pimpl->constraintsControlHessianBuffer.resize(control.size(), control.size());
-            }
-
             if (m_pimpl->constraintsGroups.size() == 0) {
                 hessian.zero();
                 return true;
@@ -1397,10 +1423,6 @@ namespace iDynTree {
                 hessian.resize(state.size(), control.size());
             }
 
-            if ((m_pimpl->constraintsMixedHessianBuffer.rows() != state.size()) || (m_pimpl->constraintsMixedHessianBuffer.cols() != control.size())) {
-                m_pimpl->constraintsMixedHessianBuffer.resize(state.size(), control.size());
-            }
-
             if (m_pimpl->constraintsGroups.size() == 0) {
                 hessian.zero();
                 return true;

From fb5676503f62d2b1d674c8c0a8341b65cd3f1a90 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 7 Feb 2019 15:25:18 +0100
Subject: [PATCH 107/194] Bumped version to account for the hessians
 availability.

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5f31c4a130f..6ea5b6b79c3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,7 +25,7 @@ set(VARS_PREFIX "iDynTree")
 
 set(${VARS_PREFIX}_MAJOR_VERSION 0)
 set(${VARS_PREFIX}_MINOR_VERSION 11)
-set(${VARS_PREFIX}_PATCH_VERSION 102)
+set(${VARS_PREFIX}_PATCH_VERSION 103)
 set(${VARS_PREFIX}_VERSION ${${VARS_PREFIX}_MAJOR_VERSION}.${${VARS_PREFIX}_MINOR_VERSION}.${${VARS_PREFIX}_PATCH_VERSION})
 
 # Pick up our CMake scripts - they are all in the cmake subdirectory.

From de7fa098a49594f30e9c0d5f38ea1e063d614137 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 8 Feb 2019 16:56:01 +0100
Subject: [PATCH 108/194] Double derivative of dynamical systems do not have
 the control as input.

This is to keep consistency with other dynamical system methods. The control input can be retrieved via the controlInput() method.
---
 .../include/iDynTree/DynamicalSystem.h        |  6 ---
 .../include/iDynTree/LinearSystem.h           |  3 --
 src/optimalcontrol/src/DynamicalSystem.cpp    |  6 +--
 src/optimalcontrol/src/ForwardEuler.cpp       |  8 ++--
 .../src/ImplicitTrapezoidal.cpp               | 45 +++++++++++--------
 src/optimalcontrol/src/LinearSystem.cpp       | 10 ++---
 .../tests/MultipleShootingTest.cpp            |  7 +--
 7 files changed, 40 insertions(+), 45 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index fe87b2f6899..d3bb14c3c8a 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -191,14 +191,12 @@ namespace optimalcontrol {
          * It is the result of \f$\frac{\partial^2 f(t, x, u)}{\partial x^2}\f$
          * @param[in] time The time at which the partial derivative is computed.
          * @param[in] state The state value at which the partial derivative is computed.
-         * @param[in] control The control value at which the partial derivative is computed.
          * @param[in] lambda The associated lagrange multipliers
          * @param[out] partialDerivative The output partial derivative.
          * @return True if successfull, false otherwise (or if not implemented).
          */
         virtual bool dynamicsSecondPartialDerivativeWRTState(double time,
                                                              const iDynTree::VectorDynSize& state,
-                                                             const iDynTree::VectorDynSize& control,
                                                              const iDynTree::VectorDynSize& lambda,
                                                              iDynTree::MatrixDynSize& partialDerivative);
 
@@ -208,14 +206,12 @@ namespace optimalcontrol {
          * It is the result of \f$\frac{\partial^2 f(t, x, u)}{\partial u^2}\f$
          * @param[in] time The time at which the partial derivative is computed.
          * @param[in] state The state value at which the partial derivative is computed.
-         * @param[in] control The control value at which the partial derivative is computed.
          * @param[in] lambda The associated lagrange multipliers
          * @param[out] partialDerivative The output partial derivative.
          * @return True if successfull, false otherwise (or if not implemented).
          */
         virtual bool dynamicsSecondPartialDerivativeWRTControl(double time,
                                                                const iDynTree::VectorDynSize& state,
-                                                               const iDynTree::VectorDynSize& control,
                                                                const iDynTree::VectorDynSize& lambda,
                                                                iDynTree::MatrixDynSize& partialDerivative);
 
@@ -226,14 +222,12 @@ namespace optimalcontrol {
          * thus it has number of rows equals to the number of states and number of cols equal to the number of control inputs.
          * @param[in] time The time at which the partial derivative is computed.
          * @param[in] state The state value at which the partial derivative is computed.
-         * @param[in] control The control value at which the partial derivative is computed.
          * @param[in] lambda The associated lagrange multipliers
          * @param[out] partialDerivative The output partial derivative.
          * @return True if successfull, false otherwise (or if not implemented).
          */
         virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double time,
                                                                     const iDynTree::VectorDynSize& state,
-                                                                    const iDynTree::VectorDynSize& control,
                                                                     const iDynTree::VectorDynSize& lambda,
                                                                     iDynTree::MatrixDynSize& partialDerivative);
 
diff --git a/src/optimalcontrol/include/iDynTree/LinearSystem.h b/src/optimalcontrol/include/iDynTree/LinearSystem.h
index 8136720b32b..2ba10ee4dca 100644
--- a/src/optimalcontrol/include/iDynTree/LinearSystem.h
+++ b/src/optimalcontrol/include/iDynTree/LinearSystem.h
@@ -76,19 +76,16 @@ namespace iDynTree {
 
             virtual bool dynamicsSecondPartialDerivativeWRTState(double time,
                                                                  const iDynTree::VectorDynSize& state,
-                                                                 const iDynTree::VectorDynSize& control,
                                                                  const iDynTree::VectorDynSize& lambda,
                                                                  iDynTree::MatrixDynSize& partialDerivative) final;
 
             virtual bool dynamicsSecondPartialDerivativeWRTControl(double time,
                                                                    const iDynTree::VectorDynSize& state,
-                                                                   const iDynTree::VectorDynSize& control,
                                                                    const iDynTree::VectorDynSize& lambda,
                                                                    iDynTree::MatrixDynSize& partialDerivative) final;
 
             virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double time,
                                                                         const iDynTree::VectorDynSize& state,
-                                                                        const iDynTree::VectorDynSize& control,
                                                                         const iDynTree::VectorDynSize& lambda,
                                                                         iDynTree::MatrixDynSize& partialDerivative) final;
 
diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp
index 8b56bd234ab..5fd9e2b717f 100644
--- a/src/optimalcontrol/src/DynamicalSystem.cpp
+++ b/src/optimalcontrol/src/DynamicalSystem.cpp
@@ -100,17 +100,17 @@ namespace iDynTree {
             return false;
         }
 
-        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &/*state*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
         {
             return false;
         }
 
-        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
         {
             return false;
         }
 
-        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &/*state*/, const iDynTree::VectorDynSize &/*lambda*/, iDynTree::MatrixDynSize &/*partialDerivative*/)
         {
             return false;
         }
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index 65aed94f113..4a6f3fec05c 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -304,13 +304,13 @@ namespace iDynTree {
                 }
 
                 if (!((m_dynamicalSystem_ptr->setControlInput(controlInputs[0])))){
-                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintJacobian", "Error while setting the control input.");
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", "Error while setting the control input.");
                     return false;
                 }
 
                 toEigen(m_lambda) = dT * toEigen(lambda);
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time, collocationPoints[0], controlInputs[0], m_lambda, m_stateHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time, collocationPoints[0], m_lambda, m_stateHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
                                 "Error while evaluating the dynamical system state second derivative.");
                     return false;
@@ -322,7 +322,7 @@ namespace iDynTree {
 
                 stateSecondDerivative[CollocationHessianIndex(1, 1)] = m_zeroNxNxBuffer;
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_controlHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time, collocationPoints[0], m_lambda, m_controlHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
                                 "Error while evaluating the dynamical system control second derivative.");
                     return false;
@@ -335,7 +335,7 @@ namespace iDynTree {
                 controlSecondDerivative[CollocationHessianIndex(1, 1)] = m_zeroNuNuBuffer;
 
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_mixedHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time, collocationPoints[0], m_lambda, m_mixedHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
                                 "Error while evaluating the dynamical system second derivative wrt state and control.");
                     return false;
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index 3f4b9d96789..f08abdcff4d 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -316,13 +316,13 @@ namespace iDynTree {
                 }
 
                 if (!((m_dynamicalSystem_ptr->setControlInput(controlInputs[0])))){
-                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintJacobian", "Error while setting the control input.");
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", "Error while setting the control input.");
                     return false;
                 }
 
                 toEigen(m_lambda) = 0.5 * dT * toEigen(lambda);
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time, collocationPoints[0], controlInputs[0], m_lambda, m_stateHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time, collocationPoints[0], m_lambda, m_stateHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
                                 "Error while evaluating the dynamical system state second derivative.");
                     return false;
@@ -332,46 +332,53 @@ namespace iDynTree {
 
                 stateSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNxBuffer;
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time + dT, collocationPoints[1], controlInputs[1], m_lambda, m_stateHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time, collocationPoints[0], m_lambda, m_controlHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
-                                "Error while evaluating the dynamical system state second derivative.");
+                                "Error while evaluating the dynamical system control second derivative.");
                     return false;
                 }
 
-                stateSecondDerivative[CollocationHessianIndex(1, 1)] = m_stateHessianBuffer;
+                controlSecondDerivative[CollocationHessianIndex(0, 0)] = m_controlHessianBuffer;
+
+                controlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNuNuBuffer;
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_controlHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time, collocationPoints[0], m_lambda, m_mixedHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
-                                "Error while evaluating the dynamical system control second derivative.");
+                                "Error while evaluating the dynamical system second derivative wrt state and control.");
                     return false;
                 }
 
-                controlSecondDerivative[CollocationHessianIndex(0, 0)] = m_controlHessianBuffer;
+                stateControlSecondDerivative[CollocationHessianIndex(0, 0)] = m_mixedHessianBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNuBuffer;
+
+                stateControlSecondDerivative[CollocationHessianIndex(1, 0)] = m_zeroNxNuBuffer;
 
-                controlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNuNuBuffer;
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time + dT, collocationPoints[1], controlInputs[1], m_lambda, m_controlHessianBuffer))) {
+                if (!((m_dynamicalSystem_ptr->setControlInput(controlInputs[1])))){
+                    reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives", "Error while setting the control input.");
+                    return false;
+                }
+
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTState(time + dT, collocationPoints[1], m_lambda, m_stateHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
-                                "Error while evaluating the dynamical system control second derivative.");
+                                "Error while evaluating the dynamical system state second derivative.");
                     return false;
                 }
 
-                controlSecondDerivative[CollocationHessianIndex(1, 1)] = m_controlHessianBuffer;
+                stateSecondDerivative[CollocationHessianIndex(1, 1)] = m_stateHessianBuffer;
 
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time, collocationPoints[0], controlInputs[0], m_lambda, m_mixedHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControl(time + dT, collocationPoints[1], m_lambda, m_controlHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
-                                "Error while evaluating the dynamical system second derivative wrt state and control.");
+                                "Error while evaluating the dynamical system control second derivative.");
                     return false;
                 }
 
-                stateControlSecondDerivative[CollocationHessianIndex(0, 0)] = m_mixedHessianBuffer;
-
-                stateControlSecondDerivative[CollocationHessianIndex(0, 1)] = m_zeroNxNuBuffer;
+                controlSecondDerivative[CollocationHessianIndex(1, 1)] = m_controlHessianBuffer;
 
-                stateControlSecondDerivative[CollocationHessianIndex(1, 0)] = m_zeroNxNuBuffer;
 
-                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time + dT, collocationPoints[1], controlInputs[1], m_lambda, m_mixedHessianBuffer))) {
+                if (!(m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControl(time + dT, collocationPoints[1], m_lambda, m_mixedHessianBuffer))) {
                     reportError(m_info.name().c_str(), "evaluateCollocationConstraintSecondDerivatives",
                                 "Error while evaluating the dynamical system second derivative wrt state and control.");
                     return false;
diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp
index 1935c5a6c65..3a4cf1c0b8d 100644
--- a/src/optimalcontrol/src/LinearSystem.cpp
+++ b/src/optimalcontrol/src/LinearSystem.cpp
@@ -243,23 +243,23 @@ namespace iDynTree {
             return true;
         }
 
-        bool LinearSystem::dynamicsSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &state, const VectorDynSize &/*control*/, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &state, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
         {
             partialDerivative.resize(state.size(), state.size());
             partialDerivative.zero();
             return true;
         }
 
-        bool LinearSystem::dynamicsSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &control, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
         {
-            partialDerivative.resize(control.size(), control.size());
+            partialDerivative.resize(controlInput().size(), controlInput().size());
             partialDerivative.zero();
             return true;
         }
 
-        bool LinearSystem::dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &state, const VectorDynSize &/*lambda*/, MatrixDynSize &partialDerivative)
         {
-            partialDerivative.resize(state.size(), control.size());
+            partialDerivative.resize(state.size(), controlInput().size());
             partialDerivative.zero();
             return true;
         }
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 30db48e501e..2942d7f6d5f 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -97,7 +97,6 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
 
     virtual bool dynamicsSecondPartialDerivativeWRTState(double /*time*/,
                                                          const iDynTree::VectorDynSize& state,
-                                                         const iDynTree::VectorDynSize& /*control*/,
                                                          const iDynTree::VectorDynSize& /*lambda*/,
                                                          iDynTree::MatrixDynSize& partialDerivative) override {
         partialDerivative.resize(state.size(), state.size());
@@ -107,20 +106,18 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
 
     virtual bool dynamicsSecondPartialDerivativeWRTControl(double /*time*/,
                                                            const iDynTree::VectorDynSize& /*state*/,
-                                                           const iDynTree::VectorDynSize& control,
                                                            const iDynTree::VectorDynSize& /*lambda*/,
                                                            iDynTree::MatrixDynSize& partialDerivative) override {
-        partialDerivative.resize(control.size(), control.size());
+        partialDerivative.resize(controlInput().size(), controlInput().size());
         partialDerivative.zero();
         return true;
     }
 
     virtual bool dynamicsSecondPartialDerivativeWRTStateControl(double /*time*/,
                                                                 const iDynTree::VectorDynSize& state,
-                                                                const iDynTree::VectorDynSize& control,
                                                                 const iDynTree::VectorDynSize& /*lambda*/,
                                                                 iDynTree::MatrixDynSize& partialDerivative) override {
-        partialDerivative.resize(state.size(), control.size());
+        partialDerivative.resize(state.size(), controlInput().size());
         partialDerivative.zero();
         return true;
     }

From 7c9545c18be6763f9e42fd7e836a01dd487e92dd Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 16 Apr 2019 11:14:42 +0200
Subject: [PATCH 109/194] Constraints and costs have their own private buffer
 for jacobians and hessians.

---
 .../src/OptimalControlProblem.cpp             | 167 +++++++++---------
 1 file changed, 85 insertions(+), 82 deletions(-)

diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index b2da48123f0..e6521b0e9fe 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -45,11 +45,27 @@ namespace iDynTree {
             std::shared_ptr<ConstraintsGroup> group_ptr;
             VectorDynSize constraintsBuffer;
             MatrixDynSize stateJacobianBuffer, controlJacobianBuffer;
+            MatrixDynSize stateHessianBuffer, controlHessianBuffer, mixedHessianBuffer;
             VectorDynSize lambdaBuffer;
             SparsityStructure stateSparsity;
             SparsityStructure controlSparsity;
             bool hasStateSparsity = false;
             bool hasControlSparsity = false;
+
+            template <typename intType>
+            void resizeBuffers(intType stateDim, intType controlDim) {
+                unsigned int nx = static_cast<unsigned int>(stateDim);
+                unsigned int nu = static_cast<unsigned int>(controlDim);
+                stateJacobianBuffer.resize(group_ptr->constraintsDimension(), nx);
+                controlJacobianBuffer.resize(group_ptr->constraintsDimension(), nu);
+                stateHessianBuffer.resize(nx, nx);
+                stateHessianBuffer.zero();
+                controlHessianBuffer.resize(nu, nu);
+                controlHessianBuffer.zero();
+                mixedHessianBuffer.resize(nx, nu);
+                mixedHessianBuffer.zero();
+            }
+
         } BufferedGroup;
 
         typedef std::shared_ptr<BufferedGroup> BufferedGroup_ptr;
@@ -58,10 +74,28 @@ namespace iDynTree {
 
         typedef struct{
             std::shared_ptr<Cost> cost;
+            MatrixDynSize stateHessianBuffer, controlHessianBuffer, mixedHessianBuffer;
+            VectorDynSize stateJacobianBuffer, controlJacobianBuffer;
             double weight;
             TimeRange timeRange;
             bool isLinear;
             bool isQuadratic;
+
+            template <typename intType>
+            void resizeBuffers(intType stateDim, intType controlDim) {
+                unsigned int nx = static_cast<unsigned int>(stateDim);
+                unsigned int nu = static_cast<unsigned int>(controlDim);
+                stateJacobianBuffer.resize(nx);
+                stateJacobianBuffer.zero();
+                controlJacobianBuffer.resize(nu);
+                controlJacobianBuffer.zero();
+                stateHessianBuffer.resize(nx, nx);
+                stateHessianBuffer.zero();
+                controlHessianBuffer.resize(nu, nu);
+                controlHessianBuffer.zero();
+                mixedHessianBuffer.resize(nx, nu);
+                mixedHessianBuffer.zero();
+            }
         } TimedCost;
 
         typedef std::map< std::string, TimedCost> CostsMap;
@@ -73,9 +107,6 @@ namespace iDynTree {
             std::shared_ptr<DynamicalSystem> dynamicalSystem;
             ConstraintsGroupsMap constraintsGroups;
             CostsMap costs;
-            VectorDynSize costStateJacobianBuffer, costControlJacobianBuffer;
-            MatrixDynSize costStateHessianBuffer, costControlHessianBuffer, costMixedHessianBuffer;
-            MatrixDynSize constraintsStateHessianBuffer, constraintsControlHessianBuffer, constraintsMixedHessianBuffer;
             bool stateLowerBounded, stateUpperBounded, controlLowerBounded, controlUpperBounded;
             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
             SparsityStructure stateSparsity;
@@ -98,6 +129,10 @@ namespace iDynTree {
                 newCost.isLinear = isLinear;
                 newCost.isQuadratic = isQuadratic;
 
+                if (dynamicalSystem){
+                    newCost.resizeBuffers(dynamicalSystem->stateSpaceSize(), dynamicalSystem->controlSpaceSize());
+                }
+
                 std::pair<CostsMap::iterator, bool> costResult;
                 costResult = costs.insert(std::pair<std::string, TimedCost>(cost->name(), newCost));
                 if(!costResult.second){
@@ -177,27 +212,13 @@ namespace iDynTree {
             }
             m_pimpl->dynamicalSystem = dynamicalSystem;
 
-            unsigned int nx = static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize());
-            unsigned int nu = static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize());
-
-            m_pimpl->costStateHessianBuffer.resize(nx, nx);
-            m_pimpl->costStateHessianBuffer.zero();
-
-            m_pimpl->costControlHessianBuffer.resize(nu, nu);
-            m_pimpl->costControlHessianBuffer.zero();
-
-            m_pimpl->costMixedHessianBuffer.resize(nx, nu);
-            m_pimpl->costMixedHessianBuffer.zero();
-
-
-            m_pimpl->constraintsStateHessianBuffer.resize(nx, nx);
-            m_pimpl->constraintsStateHessianBuffer.zero();
-
-            m_pimpl->constraintsControlHessianBuffer.resize(nu, nu);
-            m_pimpl->constraintsControlHessianBuffer.zero();
+            for (auto& cost : m_pimpl->costs){
+                cost.second.resizeBuffers(dynamicalSystem->stateSpaceSize(), dynamicalSystem->controlSpaceSize());
+            }
 
-            m_pimpl->constraintsMixedHessianBuffer.resize(nx, nu);
-            m_pimpl->constraintsMixedHessianBuffer.zero();
+            for (auto& group : m_pimpl->constraintsGroups){
+                group.second->resizeBuffers(dynamicalSystem->stateSpaceSize(), dynamicalSystem->controlSpaceSize());
+            }
 
             return true;
         }
@@ -211,27 +232,13 @@ namespace iDynTree {
             m_pimpl->dynamicalSystem = linearSystem;
             m_pimpl->systemIsLinear = true;
 
-            unsigned int nx = static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize());
-            unsigned int nu = static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize());
-
-            m_pimpl->costStateHessianBuffer.resize(nx, nx);
-            m_pimpl->costStateHessianBuffer.zero();
-
-            m_pimpl->costControlHessianBuffer.resize(nu, nu);
-            m_pimpl->costControlHessianBuffer.zero();
-
-            m_pimpl->costMixedHessianBuffer.resize(nx, nu);
-            m_pimpl->costMixedHessianBuffer.zero();
-
-
-            m_pimpl->constraintsStateHessianBuffer.resize(nx, nx);
-            m_pimpl->constraintsStateHessianBuffer.zero();
-
-            m_pimpl->constraintsControlHessianBuffer.resize(nu, nu);
-            m_pimpl->constraintsControlHessianBuffer.zero();
+            for (auto& cost : m_pimpl->costs){
+                cost.second.resizeBuffers(linearSystem->stateSpaceSize(), linearSystem->controlSpaceSize());
+            }
 
-            m_pimpl->constraintsMixedHessianBuffer.resize(nx, nu);
-            m_pimpl->constraintsMixedHessianBuffer.zero();
+            for (auto& group : m_pimpl->constraintsGroups){
+                group.second->resizeBuffers(linearSystem->stateSpaceSize(), linearSystem->controlSpaceSize());
+            }
 
             return true;
         }
@@ -257,8 +264,7 @@ namespace iDynTree {
             newGroup->group_ptr = groupOfConstraints;
             newGroup->constraintsBuffer.resize(groupOfConstraints->constraintsDimension());
             if (m_pimpl->dynamicalSystem) {
-                newGroup->stateJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->stateSpaceSize()));
-                newGroup->controlJacobianBuffer.resize(groupOfConstraints->constraintsDimension(), static_cast<unsigned int>(m_pimpl->dynamicalSystem->controlSpaceSize()));
+                newGroup->resizeBuffers(m_pimpl->dynamicalSystem->stateSpaceSize(), m_pimpl->dynamicalSystem->controlSpaceSize());
             }
             newGroup->lambdaBuffer.resize(groupOfConstraints->constraintsDimension());
 
@@ -927,31 +933,28 @@ namespace iDynTree {
                 partialDerivative.resize(state.size());
             }
 
-            if (m_pimpl->costStateJacobianBuffer.size() != state.size()) {
-                m_pimpl->costStateJacobianBuffer.resize(state.size());
-            }
-
             bool first = true;
 
             for(auto&  cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
-                    if(!cost.second.cost->costFirstPartialDerivativeWRTState(time, state, control, m_pimpl->costStateJacobianBuffer)){
+
+                    if(!cost.second.cost->costFirstPartialDerivativeWRTState(time, state, control, cost.second.stateJacobianBuffer)){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost " << cost.second.cost->name() <<".";
                         reportError("OptimalControlProblem", "costsFirstPartialDerivativeWRTState", errorMsg.str().c_str());
                         return false;
                     }
-                    if (m_pimpl->costStateJacobianBuffer.size() != state.size()){
+                    if (cost.second.stateJacobianBuffer.size() != state.size()){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating " << cost.second.cost->name() <<": " << "the jacobian size is expected to match the state dimension.";
                         reportError("OptimalControlProblem", "costsFirstPartialDerivativeWRTState", errorMsg.str().c_str());
                         return false;
                     }
                     if (first){
-                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costStateJacobianBuffer);
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(cost.second.stateJacobianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costStateJacobianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(cost.second.stateJacobianBuffer);
                     }
                 }
             }
@@ -964,31 +967,27 @@ namespace iDynTree {
                 partialDerivative.resize(control.size());
             }
 
-            if (m_pimpl->costControlJacobianBuffer.size() != control.size()) {
-                m_pimpl->costControlJacobianBuffer.resize(control.size());
-            }
-
             bool first = true;
 
             for(auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
-                    if (!cost.second.cost->costFirstPartialDerivativeWRTControl(time, state, control, m_pimpl->costControlJacobianBuffer)){
+                    if (!cost.second.cost->costFirstPartialDerivativeWRTControl(time, state, control, cost.second.controlJacobianBuffer)){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost " << cost.second.cost->name() <<".";
                         reportError("OptimalControlProblem", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
                         return false;
                     }
-                    if (m_pimpl->costControlJacobianBuffer.size() != control.size()){
+                    if (cost.second.controlJacobianBuffer.size() != control.size()){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating " << cost.second.cost->name() <<": " << "the jacobian size is expected to match the control dimension.";
                         reportError("OptimalControlProblem", "costFirstPartialDerivativeWRTControl", errorMsg.str().c_str());
                         return false;
                     }
                     if (first){
-                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costControlJacobianBuffer);
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(cost.second.controlJacobianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costControlJacobianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(cost.second.controlJacobianBuffer);
                     }
                 }
             }
@@ -1005,24 +1004,25 @@ namespace iDynTree {
 
             for (auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
-                    if (!cost.second.cost->costSecondPartialDerivativeWRTState(time, state, control, m_pimpl->costStateHessianBuffer)){
+
+                    if (!cost.second.cost->costSecondPartialDerivativeWRTState(time, state, control, cost.second.stateHessianBuffer)){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost " << cost.second.cost->name() <<".";
                         reportError("OptimalControlProblem", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
                         return false;
                     }
 
-                    if ((m_pimpl->costStateHessianBuffer.rows() != state.size()) || (m_pimpl->costStateHessianBuffer.cols() != state.size())){
+                    if ((cost.second.stateHessianBuffer.rows() != state.size()) || (cost.second.stateHessianBuffer.cols() != state.size())){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating " << cost.second.cost->name() <<": " << "the hessian size is expected to be a square matrix matching the state dimension.";
                         reportError("OptimalControlProblem", "costSecondPartialDerivativeWRTState", errorMsg.str().c_str());
                         return false;
                     }
                     if (first){
-                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costStateHessianBuffer);
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(cost.second.stateHessianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costStateHessianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(cost.second.stateHessianBuffer);
                     }
                 }
             }
@@ -1039,14 +1039,15 @@ namespace iDynTree {
 
             for (auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
-                    if (!cost.second.cost->costSecondPartialDerivativeWRTControl(time, state, control, m_pimpl->costControlHessianBuffer)){
+
+                    if (!cost.second.cost->costSecondPartialDerivativeWRTControl(time, state, control, cost.second.controlHessianBuffer)){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost " << cost.second.cost->name() <<".";
                         reportError("OptimalControlProblem", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
                         return false;
                     }
 
-                    if ((m_pimpl->costControlHessianBuffer.rows() != control.size()) || (m_pimpl->costControlHessianBuffer.cols() != control.size())){
+                    if ((cost.second.controlHessianBuffer.rows() != control.size()) || (cost.second.controlHessianBuffer.cols() != control.size())){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating " << cost.second.cost->name() <<": " << "the hessian size is expected to be a square matrix matching the control dimension.";
                         reportError("OptimalControlProblem", "costSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
@@ -1054,10 +1055,10 @@ namespace iDynTree {
                     }
 
                     if (first){
-                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costControlHessianBuffer);
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(cost.second.controlHessianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costControlHessianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(cost.second.controlHessianBuffer);
                     }
                 }
             }
@@ -1074,14 +1075,15 @@ namespace iDynTree {
 
             for (auto& cost : m_pimpl->costs){
                 if (cost.second.timeRange.isInRange(time)){
-                    if (!cost.second.cost->costSecondPartialDerivativeWRTStateControl(time, state, control, m_pimpl->costMixedHessianBuffer)){
+
+                    if (!cost.second.cost->costSecondPartialDerivativeWRTStateControl(time, state, control, cost.second.mixedHessianBuffer)){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating cost " << cost.second.cost->name() <<".";
                         reportError("OptimalControlProblem", "costSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
                         return false;
                     }
 
-                    if ((m_pimpl->costMixedHessianBuffer.rows() != state.size()) || (m_pimpl->costMixedHessianBuffer.cols() != control.size())){
+                    if ((cost.second.mixedHessianBuffer.rows() != state.size()) || (cost.second.mixedHessianBuffer.cols() != control.size())){
                         std::ostringstream errorMsg;
                         errorMsg << "Error while evaluating " << cost.second.cost->name() <<": " << "the hessian size is expected to have as many rows as the state dimension and a number of columns matching the control dimension.";
                         reportError("OptimalControlProblem", "costSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
@@ -1089,10 +1091,10 @@ namespace iDynTree {
                     }
 
                     if (first){
-                        toEigen(partialDerivative) = cost.second.weight * toEigen(m_pimpl->costMixedHessianBuffer);
+                        toEigen(partialDerivative) = cost.second.weight * toEigen(cost.second.mixedHessianBuffer);
                         first = false;
                     } else {
-                        toEigen(partialDerivative) += cost.second.weight * toEigen(m_pimpl->costMixedHessianBuffer);
+                        toEigen(partialDerivative) += cost.second.weight * toEigen(cost.second.mixedHessianBuffer);
                     }
                 }
             }
@@ -1366,17 +1368,18 @@ namespace iDynTree {
 
             for (auto& group : m_pimpl->constraintsGroups){
                 toEigen(group.second->lambdaBuffer) = toEigen(lambda).segment(offset, group.second->group_ptr->constraintsDimension());
-                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTState(time, state, control, group.second->lambdaBuffer, m_pimpl->constraintsStateHessianBuffer)){
+
+                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTState(time, state, control, group.second->lambdaBuffer, group.second->stateHessianBuffer)){
                     std::ostringstream errorMsg;
                     errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
                     reportError("OptimalControlProblem", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
                     return false;
                 }
                 if (first){
-                    toEigen(hessian) = toEigen(m_pimpl->constraintsStateHessianBuffer);
+                    toEigen(hessian) = toEigen(group.second->stateHessianBuffer);
                     first = false;
                 } else {
-                    toEigen(hessian) += toEigen(m_pimpl->constraintsStateHessianBuffer);
+                    toEigen(hessian) += toEigen(group.second->stateHessianBuffer);
                 }
                 offset += group.second->group_ptr->constraintsDimension();
             }
@@ -1400,17 +1403,17 @@ namespace iDynTree {
 
             for (auto& group : m_pimpl->constraintsGroups){
                 toEigen(group.second->lambdaBuffer) = toEigen(lambda).segment(offset, group.second->group_ptr->constraintsDimension());
-                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTControl(time, state, control, group.second->lambdaBuffer, m_pimpl->constraintsControlHessianBuffer)){
+                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTControl(time, state, control, group.second->lambdaBuffer, group.second->controlHessianBuffer)){
                     std::ostringstream errorMsg;
                     errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
                     reportError("OptimalControlProblem", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
                     return false;
                 }
                 if (first){
-                    toEigen(hessian) = toEigen(m_pimpl->constraintsControlHessianBuffer);
+                    toEigen(hessian) = toEigen(group.second->controlHessianBuffer);
                     first = false;
                 } else {
-                    toEigen(hessian) += toEigen(m_pimpl->constraintsControlHessianBuffer);
+                    toEigen(hessian) += toEigen(group.second->controlHessianBuffer);
                 }
                 offset += group.second->group_ptr->constraintsDimension();
             }
@@ -1434,17 +1437,17 @@ namespace iDynTree {
             for (auto& group : m_pimpl->constraintsGroups){
                 toEigen(group.second->lambdaBuffer) = toEigen(lambda).segment(offset, group.second->group_ptr->constraintsDimension());
 
-                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTStateControl(time, state, control, group.second->lambdaBuffer, m_pimpl->constraintsMixedHessianBuffer)){
+                if (! group.second->group_ptr->constraintSecondPartialDerivativeWRTStateControl(time, state, control, group.second->lambdaBuffer, group.second->mixedHessianBuffer)){
                     std::ostringstream errorMsg;
                     errorMsg << "Error while evaluating constraint " << group.second->group_ptr->name() <<".";
                     reportError("OptimalControlProblem", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
                     return false;
                 }
                 if (first){
-                    toEigen(hessian) = toEigen(m_pimpl->constraintsMixedHessianBuffer);
+                    toEigen(hessian) = toEigen(group.second->mixedHessianBuffer);
                     first = false;
                 } else {
-                    toEigen(hessian) += toEigen(m_pimpl->constraintsMixedHessianBuffer);
+                    toEigen(hessian) += toEigen(group.second->mixedHessianBuffer);
                 }
                 offset += group.second->group_ptr->constraintsDimension();
             }

From 11fe24831885fcb53bd01879427d287416798a89 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 1 May 2019 00:57:02 +0200
Subject: [PATCH 110/194] Fixed hessian related to constraints on initial
 state.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 9 ---------
 1 file changed, 9 deletions(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index a4b9c7479aa..855bbb35e40 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -2039,15 +2039,6 @@ namespace iDynTree {
 
                             lambdaConstraints = fullLambda.segment(constraintIndex, nc);
 
-                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTStateControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_lambdaConstraints, m_stateControlHessianBuffer))){
-                                std::ostringstream errorMsg;
-                                errorMsg << "Error while evaluating the constraints hessian wrt state and control at time " << mesh->time << ".";
-                                reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
-                                return false;
-                            }
-
-                            setHessianBlockAndItsTranspose(hessian, m_stateControlHessianBuffer, mesh->stateIndex, mesh->controlIndex);
-
                             if (!(m_ocproblem->constraintSecondPartialDerivativeWRTControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
                                 std::ostringstream errorMsg;
                                 errorMsg << "Error while evaluating the constraints hessian wrt control at time " << mesh->time << ".";

From cfa9e51b0eadb44e6ae88c9d7ca1f708327cf36e Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 8 May 2019 18:05:51 +0200
Subject: [PATCH 111/194] Added possibility to specify the hessian sparsity
 structure.

---
 .../include/iDynTree/Constraint.h             |  31 +-
 .../include/iDynTree/ConstraintsGroup.h       |  27 ++
 src/optimalcontrol/include/iDynTree/Cost.h    |  29 ++
 .../include/iDynTree/DynamicalSystem.h        |  27 ++
 .../include/iDynTree/Integrator.h             |  12 +
 .../iDynTree/Integrators/ForwardEuler.h       |  16 +-
 .../Integrators/ImplicitTrapezoidal.h         |  16 +-
 .../include/iDynTree/L2NormCost.h             |   6 +
 .../include/iDynTree/LinearConstraint.h       |   6 +
 .../include/iDynTree/LinearSystem.h           |   6 +
 .../include/iDynTree/OptimalControlProblem.h  |  22 +-
 .../include/iDynTree/QuadraticCost.h          |   5 +
 .../include/iDynTree/QuadraticLikeCost.h      |  15 +
 .../include/iDynTree/SparsityStructure.h      |   2 +
 src/optimalcontrol/src/Constraint.cpp         |  15 +
 src/optimalcontrol/src/ConstraintsGroup.cpp   | 133 +++++--
 src/optimalcontrol/src/Cost.cpp               |  25 +-
 src/optimalcontrol/src/DynamicalSystem.cpp    |  15 +
 src/optimalcontrol/src/ForwardEuler.cpp       |  60 +++-
 .../src/ImplicitTrapezoidal.cpp               |  62 +++-
 src/optimalcontrol/src/Integrator.cpp         |  25 ++
 src/optimalcontrol/src/L2NormCost.cpp         |  43 ++-
 src/optimalcontrol/src/LinearConstraint.cpp   |  18 +
 src/optimalcontrol/src/LinearCost.cpp         |   6 +-
 src/optimalcontrol/src/LinearSystem.cpp       |  18 +
 .../src/MultipleShootingSolver.cpp            | 326 +++++++++++-------
 .../src/OptimalControlProblem.cpp             | 316 ++++++++++++++---
 src/optimalcontrol/src/QuadraticCost.cpp      |  26 +-
 src/optimalcontrol/src/QuadraticLikeCost.cpp  |  30 ++
 src/optimalcontrol/src/SparsityStructure.cpp  |  14 +
 src/optimalcontrol/tests/OCProblemTest.cpp    |   4 +-
 31 files changed, 1137 insertions(+), 219 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/Constraint.h b/src/optimalcontrol/include/iDynTree/Constraint.h
index e01c1ee2e20..30d971773a9 100644
--- a/src/optimalcontrol/include/iDynTree/Constraint.h
+++ b/src/optimalcontrol/include/iDynTree/Constraint.h
@@ -167,7 +167,7 @@ namespace iDynTree {
              * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state jacobian
              *
              * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, stateDimension) respectively.
-             * @param stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
+             * @param stateSparsity Sparsity structure of the partial derivative of the constraint wrt state variables.
              * @return true if the sparsity is available. False otherwise.
              */
             virtual bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
@@ -176,7 +176,7 @@ namespace iDynTree {
              * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control jacobian
              *
              * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
-             * @param controlSparsity Sparsity structure of the partial derivative of the jacobian wrt control variables.
+             * @param controlSparsity Sparsity structure of the partial derivative of the constraint wrt control variables.
              * @return true if the sparsity is available. False otherwise.
              */
             virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
@@ -235,6 +235,33 @@ namespace iDynTree {
                                                                           const VectorDynSize& lambda,
                                                                           MatrixDynSize& hessian);
 
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, stateDimension) respectively.
+             * @param stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool constraintSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the mixed hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, controlDimension) respectively.
+             * @param stateControlSparsity Sparsity structure of the partial derivative of the jacobian wrt state and control variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool constraintSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
+             * @param controlSparsity Sparsity structure of the partial derivative of the jacobian wrt control variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool constraintSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
+
 
         private:
             size_t m_constraintSize;
diff --git a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
index c61b9266566..eb09fcd43d6 100644
--- a/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
+++ b/src/optimalcontrol/include/iDynTree/ConstraintsGroup.h
@@ -260,6 +260,33 @@ namespace iDynTree {
                                                                   const VectorDynSize& lambda,
                                                                   MatrixDynSize& hessian);
 
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, stateDimension) respectively.
+             * @param stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            bool constraintsSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the mixed hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, controlDimension) respectively.
+             * @param stateControlSparsity Sparsity structure of the partial derivative of the jacobian wrt state and control variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            bool constraintsSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
+             * @param controlSparsity Sparsity structure of the partial derivative of the jacobian wrt control variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            bool constraintsSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
+
             /**
              * @brief Flag returning true if the group is an "AnyTime" group.
              * An "AnyTime" group contains only one constraint which is always enabled. It corresponds to a simple constraint.
diff --git a/src/optimalcontrol/include/iDynTree/Cost.h b/src/optimalcontrol/include/iDynTree/Cost.h
index 8cdb65bc02f..c7ba56bfcdd 100644
--- a/src/optimalcontrol/include/iDynTree/Cost.h
+++ b/src/optimalcontrol/include/iDynTree/Cost.h
@@ -18,6 +18,8 @@
 #define IDYNTREE_OPTIMALCONTROL_COST_H
 
 #include <string>
+#include <iDynTree/SparsityStructure.h>
+
 
 namespace iDynTree {
 
@@ -144,6 +146,33 @@ namespace iDynTree {
                                                                     const iDynTree::VectorDynSize& control,
                                                                     iDynTree::MatrixDynSize& partialDerivative);
 
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, stateDimension) respectively.
+             * @param[out] stateSparsity Sparsity structure of the partial derivative of the gradient wrt state variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool costSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the mixed hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, controlDimension) respectively.
+             * @param[out] stateControlSparsity Sparsity structure of the partial derivative of the gradient wrt state and control variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool costSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity);
+
+            /**
+             * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control hessian
+             *
+             * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
+             * @param[out] controlSparsity Sparsity structure of the partial derivative of the gradient wrt control variables.
+             * @return true if the sparsity is available. False otherwise.
+             */
+            virtual bool costSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
+
 
         private:
 
diff --git a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
index d3bb14c3c8a..e92cc4f92b8 100644
--- a/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
+++ b/src/optimalcontrol/include/iDynTree/DynamicalSystem.h
@@ -231,6 +231,33 @@ namespace optimalcontrol {
                                                                     const iDynTree::VectorDynSize& lambda,
                                                                     iDynTree::MatrixDynSize& partialDerivative);
 
+        /**
+        * @brief Returns the set of nonzeros elements in terms of row and colun index, in the state hessian
+        *
+        * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, stateDimension) respectively.
+        * @param[out] stateSparsity Sparsity structure of the partial derivative of the jacobian wrt state variables.
+        * @return true if the sparsity is available. False otherwise.
+        */
+        virtual bool dynamicsSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
+
+        /**
+        * @brief Returns the set of nonzeros elements in terms of row and colun index, in the mixed hessian
+        *
+        * @warning No check is performed in the indeces. They need to be in the range [0, stateDimension) and [0, controlDimension) respectively.
+        * @param[out] stateControlSparsity Sparsity structure of the partial derivative of the jacobian wrt state and control variables.
+        * @return true if the sparsity is available. False otherwise.
+        */
+        virtual bool dynamicsSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity);
+
+        /**
+        * @brief Returns the set of nonzeros elements in terms of row and colun index, in the control hessian
+        *
+        * @warning No check is performed in the indeces. They need to be in the range [0, constraintDimension) and [0, controlDimension) respectively.
+        * @param[out] controlSparsity Sparsity structure of the partial derivative of the jacobian wrt control variables.
+        * @return true if the sparsity is available. False otherwise.
+        */
+        virtual bool dynamicsSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
+
     private:
         size_t m_stateSize;
         size_t m_controlSize;
diff --git a/src/optimalcontrol/include/iDynTree/Integrator.h b/src/optimalcontrol/include/iDynTree/Integrator.h
index eac8d4a6a30..4f02d9211bb 100644
--- a/src/optimalcontrol/include/iDynTree/Integrator.h
+++ b/src/optimalcontrol/include/iDynTree/Integrator.h
@@ -118,10 +118,16 @@ namespace optimalcontrol {
                 CollocationHessianIndex(size_t first, size_t second);
 
                 bool operator< (const CollocationHessianIndex& rhs) const;
+
+                size_t first() const;
+
+                size_t second() const;
             };
 
             using CollocationHessianMap = std::map<CollocationHessianIndex, MatrixDynSize>;
 
+            using CollocationHessianSparsityMap = std::map<CollocationHessianIndex, SparsityStructure>;
+
             /**
              * @warning This class is still in active development, and so API interface can change between iDynTree versions.
              * \ingroup iDynTreeExperimental
@@ -263,6 +269,12 @@ namespace optimalcontrol {
                                                                             CollocationHessianMap& controlSecondDerivative,
                                                                             CollocationHessianMap& stateControlSecondDerivative);
 
+                virtual bool getCollocationConstraintSecondDerivativeWRTStateSparsity(CollocationHessianSparsityMap& stateDerivativeSparsity);
+
+                virtual bool getCollocationConstraintSecondDerivativeWRTControlSparsity(CollocationHessianSparsityMap& controlDerivativeSparsity);
+
+                virtual bool getCollocationConstraintSecondDerivativeWRTStateControlSparsity(CollocationHessianSparsityMap& stateControlDerivativeSparsity);
+
                 const IntegratorInfo& info() const;
 
             protected:
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
index 650d7e80742..c37cc1484fc 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ForwardEuler.h
@@ -38,10 +38,16 @@ namespace iDynTree {
                 MatrixDynSize m_identity, m_zeroNxNxBuffer, m_zeroNuNuBuffer, m_zeroNxNuBuffer;
                 MatrixDynSize m_stateHessianBuffer, m_controlHessianBuffer, m_mixedHessianBuffer;
                 VectorDynSize m_lambda;
-                bool m_hasStateSparsity = false;
-                bool m_hasControlSparsity = false;
+                bool m_hasStateJacobianSparsity = false;
+                bool m_hasControlJacobianSparsity = false;
                 std::vector<SparsityStructure> m_stateJacobianSparsity;
                 std::vector<SparsityStructure> m_controlJacobianSparsity;
+                bool m_hasStateHessianSparsity = false;
+                bool m_hasStateControlHessianSparsity = false;
+                bool m_hasControlHessianSparsity = false;
+                CollocationHessianSparsityMap m_stateHessianSparsity;
+                CollocationHessianSparsityMap m_stateControlHessianSparsity;
+                CollocationHessianSparsityMap m_controlHessianSparsity;
 
                 bool allocateBuffers() override;
 
@@ -75,6 +81,12 @@ namespace iDynTree {
                                                                             CollocationHessianMap& controlSecondDerivative,
                                                                             CollocationHessianMap& stateControlSecondDerivative) override;
 
+                virtual bool getCollocationConstraintSecondDerivativeWRTStateSparsity(CollocationHessianSparsityMap& stateDerivativeSparsity) override;
+
+                virtual bool getCollocationConstraintSecondDerivativeWRTControlSparsity(CollocationHessianSparsityMap& controlDerivativeSparsity) override;
+
+                virtual bool getCollocationConstraintSecondDerivativeWRTStateControlSparsity(CollocationHessianSparsityMap& stateControlDerivativeSparsity) override;
+
             };
         }
     }
diff --git a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
index d26965523b1..1fbfdeabd9e 100644
--- a/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
+++ b/src/optimalcontrol/include/iDynTree/Integrators/ImplicitTrapezoidal.h
@@ -38,10 +38,16 @@ namespace iDynTree {
                 MatrixDynSize m_zeroNxNxBuffer, m_zeroNuNuBuffer, m_zeroNxNuBuffer;
                 MatrixDynSize m_stateHessianBuffer, m_controlHessianBuffer, m_mixedHessianBuffer;
                 VectorDynSize m_lambda;
-                bool m_hasStateSparsity = false;
-                bool m_hasControlSparsity = false;
+                bool m_hasStateJacobianSparsity = false;
+                bool m_hasControlJacobianSparsity = false;
                 std::vector<SparsityStructure> m_stateJacobianSparsity;
                 std::vector<SparsityStructure> m_controlJacobianSparsity;
+                bool m_hasStateHessianSparsity = false;
+                bool m_hasStateControlHessianSparsity = false;
+                bool m_hasControlHessianSparsity = false;
+                CollocationHessianSparsityMap m_stateHessianSparsity;
+                CollocationHessianSparsityMap m_stateControlHessianSparsity;
+                CollocationHessianSparsityMap m_controlHessianSparsity;
 
                 virtual bool allocateBuffers() override;
 
@@ -75,6 +81,12 @@ namespace iDynTree {
                                                                             CollocationHessianMap& controlSecondDerivative,
                                                                             CollocationHessianMap& stateControlSecondDerivative) override;
 
+                virtual bool getCollocationConstraintSecondDerivativeWRTStateSparsity(CollocationHessianSparsityMap& stateDerivativeSparsity) override;
+
+                virtual bool getCollocationConstraintSecondDerivativeWRTControlSparsity(CollocationHessianSparsityMap& controlDerivativeSparsity) override;
+
+                virtual bool getCollocationConstraintSecondDerivativeWRTStateControlSparsity(CollocationHessianSparsityMap& stateControlDerivativeSparsity) override;
+
             };
 
         }
diff --git a/src/optimalcontrol/include/iDynTree/L2NormCost.h b/src/optimalcontrol/include/iDynTree/L2NormCost.h
index c35b2c2b704..259b51f0642 100644
--- a/src/optimalcontrol/include/iDynTree/L2NormCost.h
+++ b/src/optimalcontrol/include/iDynTree/L2NormCost.h
@@ -99,6 +99,12 @@ namespace iDynTree {
                                                                     const iDynTree::VectorDynSize& state,
                                                                     const iDynTree::VectorDynSize& control,
                                                                     iDynTree::MatrixDynSize& partialDerivative) final;
+
+            virtual bool costSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) final;
+
+            virtual bool costSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity) final;
+
+            virtual bool costSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
         };
 
     }
diff --git a/src/optimalcontrol/include/iDynTree/LinearConstraint.h b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
index 07d4a9b8712..d0aa3a9aa1c 100644
--- a/src/optimalcontrol/include/iDynTree/LinearConstraint.h
+++ b/src/optimalcontrol/include/iDynTree/LinearConstraint.h
@@ -97,6 +97,12 @@ namespace iDynTree {
                                                                           const VectorDynSize& lambda,
                                                                           MatrixDynSize& hessian) final;
 
+            virtual bool constraintSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) final;
+
+            virtual bool constraintSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity) final;
+
+            virtual bool constraintSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
+
         private:
             class LinearConstraintImplementation;
             LinearConstraintImplementation* m_pimpl;
diff --git a/src/optimalcontrol/include/iDynTree/LinearSystem.h b/src/optimalcontrol/include/iDynTree/LinearSystem.h
index 2ba10ee4dca..61c0143c135 100644
--- a/src/optimalcontrol/include/iDynTree/LinearSystem.h
+++ b/src/optimalcontrol/include/iDynTree/LinearSystem.h
@@ -89,6 +89,12 @@ namespace iDynTree {
                                                                         const iDynTree::VectorDynSize& lambda,
                                                                         iDynTree::MatrixDynSize& partialDerivative) final;
 
+            virtual bool dynamicsSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) final;
+
+            virtual bool dynamicsSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity) final;
+
+            virtual bool dynamicsSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
+
 
         private:
             class LinearSystemPimpl;
diff --git a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
index aacc421f409..169c615fa1f 100644
--- a/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
+++ b/src/optimalcontrol/include/iDynTree/OptimalControlProblem.h
@@ -224,6 +224,12 @@ namespace iDynTree {
                                                              const VectorDynSize& control,
                                                              MatrixDynSize& partialDerivative);
 
+            bool costsSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity); //returns false if something goes wrong
+
+            bool costsSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity); //returns false if something goes wrong
+
+            bool costsSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity); //returns false if something goes wrong
+
             bool constraintsEvaluation(double time, const VectorDynSize& state, const VectorDynSize& control, VectorDynSize& constraintsValue);
 
             bool getConstraintsUpperBound(double time, double infinity, VectorDynSize& upperBound); //returns false if something goes wrong
@@ -242,28 +248,34 @@ namespace iDynTree {
                                                const VectorDynSize& control,
                                                MatrixDynSize& jacobian);
 
-            bool constraintJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity);
+            bool constraintsJacobianWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity); //returns false if something goes wrong
 
-            bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity);
+            bool constraintsJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity); //returns false if something goes wrong
 
-            bool constraintSecondPartialDerivativeWRTState(double time,
+            bool constraintsSecondPartialDerivativeWRTState(double time,
                                                            const VectorDynSize& state,
                                                            const VectorDynSize& control,
                                                            const VectorDynSize& lambda,
                                                            MatrixDynSize& hessian);
 
-            bool constraintSecondPartialDerivativeWRTControl(double time,
+            bool constraintsSecondPartialDerivativeWRTControl(double time,
                                                              const VectorDynSize& state,
                                                              const VectorDynSize& control,
                                                              const VectorDynSize& lambda,
                                                              MatrixDynSize& hessian);
 
-            bool constraintSecondPartialDerivativeWRTStateControl(double time,
+            bool constraintsSecondPartialDerivativeWRTStateControl(double time,
                                                                   const VectorDynSize& state,
                                                                   const VectorDynSize& control,
                                                                   const VectorDynSize& lambda,
                                                                   MatrixDynSize& hessian);
 
+            bool constraintsSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity); //returns false if something goes wrong
+
+            bool constraintsSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity); //returns false if something goes wrong
+
+            bool constraintsSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity); //returns false if something goes wrong
+
         private:
             class OptimalControlProblemPimpl;
             OptimalControlProblemPimpl* m_pimpl;
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticCost.h b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
index 84253c1349a..2fa27f2bdbf 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticCost.h
@@ -53,6 +53,11 @@ namespace iDynTree {
 
            bool setCostBias(std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingStateCostBias,
                             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> timeVaryingControlCostBias);
+
+           bool setStateHessianSparsity(const SparsityStructure& stateSparsity);
+
+           bool setControlHessianSparsity(const SparsityStructure& controlSparsity);
+
        };
     }
 }
diff --git a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
index 05e6c5bbfbd..40439936abf 100644
--- a/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
+++ b/src/optimalcontrol/include/iDynTree/QuadraticLikeCost.h
@@ -67,6 +67,12 @@ namespace iDynTree {
                                                                    const iDynTree::VectorDynSize& control,
                                                                    iDynTree::MatrixDynSize& partialDerivative) final;
 
+           virtual bool costSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure& stateSparsity) final;
+
+           virtual bool costSecondPartialDerivativeWRTStateControlSparsity(iDynTree::optimalcontrol::SparsityStructure& stateControlSparsity) final;
+
+           virtual bool costSecondPartialDerivativeWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) final;
+
        protected:
            QuadraticLikeCost(const std::string& costName);  //this class serves only as base class
 
@@ -76,6 +82,15 @@ namespace iDynTree {
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingMatrix> m_timeVaryingControlHessian;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> m_timeVaryingControlGradient;
            std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingDouble> m_timeVaryingControlCostBias;
+
+           bool m_hasSecondPartialDerivativeWRTStateSparsity;
+           bool m_hasSecondPartialDerivativeWRTControlSparsity;
+           bool m_hasSecondPartialDerivativeWRTStateControlSparsity;
+
+           SparsityStructure m_secondPartialDerivativeWRTStateSparsity;
+           SparsityStructure m_secondPartialDerivativeWRTControlSparsity;
+           SparsityStructure m_secondPartialDerivativeWRTStateControlSparsity;
+
        };
     }
 }
diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
index aa0c63e09f9..a0e122f8cae 100644
--- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -44,6 +44,8 @@ class iDynTree::optimalcontrol::SparsityStructure {
 
     void addIdentityBlock(size_t startRow, size_t startColumn, size_t dimension);
 
+    bool addBlock(size_t startRow, size_t startColumn, const SparsityStructure& other);
+
     void addNonZeroIfNotPresent(size_t newRow, size_t newCol);
 
     bool isValuePresent(size_t row, size_t col) const;
diff --git a/src/optimalcontrol/src/Constraint.cpp b/src/optimalcontrol/src/Constraint.cpp
index 6909f03dcf0..3289acd5a27 100644
--- a/src/optimalcontrol/src/Constraint.cpp
+++ b/src/optimalcontrol/src/Constraint.cpp
@@ -160,5 +160,20 @@ namespace iDynTree {
             return false;
         }
 
+        bool Constraint::constraintSecondPartialDerivativeWRTStateSparsity(SparsityStructure &/*stateSparsity*/)
+        {
+            return false;
+        }
+
+        bool Constraint::constraintSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &/*stateControlSparsity*/)
+        {
+            return false;
+        }
+
+        bool Constraint::constraintSecondPartialDerivativeWRTControlSparsity(SparsityStructure &/*controlSparsity*/)
+        {
+            return false;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/ConstraintsGroup.cpp b/src/optimalcontrol/src/ConstraintsGroup.cpp
index ca2a1298041..8639fa2863e 100644
--- a/src/optimalcontrol/src/ConstraintsGroup.cpp
+++ b/src/optimalcontrol/src/ConstraintsGroup.cpp
@@ -49,14 +49,20 @@ namespace optimalcontrol {
         public:
             GroupOfConstraintsMap group;
             std::vector<TimedConstraint_ptr> orderedIntervals;
-            SparsityStructure groupStateSparsity;
-            SparsityStructure groupControlSparsity;
+            SparsityStructure groupStateJacobianSparsity;
+            SparsityStructure groupControlJacobianSparsity;
+            SparsityStructure groupStateHessianSparsity;
+            SparsityStructure groupControlHessianSparsity;
+            SparsityStructure groupMixedHessianSparsity;
             std::string name;
             unsigned int maxConstraintSize;
             std::vector<TimeRange> timeRanges;
             bool isLinearGroup = true;
-            bool stateSparsityProvided = true;
-            bool controlSparsityProvided = true;
+            bool stateJacobianSparsityProvided = true;
+            bool controlJacobianSparsityProvided = true;
+            bool stateHessianSparsityProvided = true;
+            bool controlHessianSparsityProvided = true;
+            bool mixedHessianSparsityProvided = true;
 
 
             std::vector<TimedConstraint_ptr>::reverse_iterator findActiveConstraint(double time){
@@ -126,34 +132,80 @@ namespace optimalcontrol {
                 orderedIntervals.push_back(result.first->second); //register the time range in order to have the constraints ordered by init time. result.first->second is the TimedConstraint_ptr of the newly inserted TimedConstraint.
                 std::sort(orderedIntervals.begin(), orderedIntervals.end(), [](const TimedConstraint_ptr&a, const TimedConstraint_ptr&b) { return a->timeRange < b->timeRange;}); //reorder the vector
 
-                SparsityStructure newConstraintStateSparsity, newConstraintControlSparsity;
+                {
+                    SparsityStructure newConstraintStateJacobianSparsity, newConstraintControlJacobianSparsity;
 
-                if (stateSparsityProvided
-                        && constraint->constraintJacobianWRTStateSparsity(newConstraintStateSparsity)) {
+                    if (stateJacobianSparsityProvided
+                        && constraint->constraintJacobianWRTStateSparsity(newConstraintStateJacobianSparsity)) {
 
-                    if (!newConstraintStateSparsity.isValid()) {
-                        reportError("ConstraintsGroup", "addConstraint", "The state sparsity is provided but the dimension of the two vectors do not match.");
+                        if (!newConstraintStateJacobianSparsity.isValid()) {
+                            reportError("ConstraintsGroup", "addConstraint", "The state sparsity is provided but the dimension of the two vectors do not match.");
+                            return false;
+                        }
+
+                        groupStateJacobianSparsity.merge(newConstraintStateJacobianSparsity);
+
+                    } else {
+                        stateJacobianSparsityProvided = false;
+                    }
+
+                    if (controlJacobianSparsityProvided
+                        && constraint->constraintJacobianWRTControlSparsity(newConstraintControlJacobianSparsity)) {
+
+                        if (!newConstraintControlJacobianSparsity.isValid()) {
+                            reportError("ConstraintsGroup", "addConstraint", "The control sparsity is provided but the dimension of the two vectors do not match.");
+                            return false;
+                        }
+
+                        groupControlJacobianSparsity.merge(newConstraintControlJacobianSparsity);
+
+                    } else {
+                        controlJacobianSparsityProvided = false;
+                    }
+                }
+
+                SparsityStructure newConstraintStateHessianSparsity, newConstraintControlHessianSparsity, newConstraintMixedHessianSparsity;
+
+                if (stateHessianSparsityProvided
+                    && constraint->constraintSecondPartialDerivativeWRTStateSparsity(newConstraintStateHessianSparsity)) {
+
+                    if (!newConstraintStateHessianSparsity.isValid()) {
+                        reportError("ConstraintsGroup", "addConstraint", "The state hessian sparsity is provided but the dimension of the two vectors do not match.");
                         return false;
                     }
 
-                    groupStateSparsity.merge(newConstraintStateSparsity);
+                    groupStateHessianSparsity.merge(newConstraintStateHessianSparsity);
 
                 } else {
-                    stateSparsityProvided = false;
+                    stateHessianSparsityProvided = false;
                 }
 
-                if (controlSparsityProvided
-                        && constraint->constraintJacobianWRTControlSparsity(newConstraintControlSparsity)) {
+                if (controlHessianSparsityProvided
+                    && constraint->constraintSecondPartialDerivativeWRTControlSparsity(newConstraintControlHessianSparsity)) {
 
-                    if (!newConstraintControlSparsity.isValid()) {
-                        reportError("ConstraintsGroup", "addConstraint", "The control sparsity is provided but the dimension of the two vectors do not match.");
+                    if (!newConstraintControlHessianSparsity.isValid()) {
+                        reportError("ConstraintsGroup", "addConstraint", "The control hessian sparsity is provided but the dimension of the two vectors do not match.");
                         return false;
                     }
 
-                    groupControlSparsity.merge(newConstraintControlSparsity);
+                    groupControlHessianSparsity.merge(newConstraintControlHessianSparsity);
 
                 } else {
-                    controlSparsityProvided = false;
+                    controlHessianSparsityProvided = false;
+                }
+
+                if (mixedHessianSparsityProvided
+                    && constraint->constraintSecondPartialDerivativeWRTStateControlSparsity(newConstraintMixedHessianSparsity)) {
+
+                    if (!newConstraintMixedHessianSparsity.isValid()) {
+                        reportError("ConstraintsGroup", "addConstraint", "The state control hessian sparsity is provided but the dimension of the two vectors do not match.");
+                        return false;
+                    }
+
+                    groupMixedHessianSparsity.merge(newConstraintMixedHessianSparsity);
+
+                } else {
+                    mixedHessianSparsityProvided = false;
                 }
 
                 return true;
@@ -551,22 +603,22 @@ namespace optimalcontrol {
 
         bool ConstraintsGroup::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity) const
         {
-            if (!(m_pimpl->stateSparsityProvided)) {
+            if (!(m_pimpl->stateJacobianSparsityProvided)) {
                 return false;
             }
 
-            stateSparsity = m_pimpl->groupStateSparsity;
+            stateSparsity = m_pimpl->groupStateJacobianSparsity;
 
             return true;
         }
 
         bool ConstraintsGroup::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity) const
         {
-            if (!(m_pimpl->controlSparsityProvided)) {
+            if (!(m_pimpl->controlJacobianSparsityProvided)) {
                 return false;
             }
 
-            controlSparsity = m_pimpl->groupControlSparsity;
+            controlSparsity = m_pimpl->groupControlJacobianSparsity;
 
             return true;
         }
@@ -609,7 +661,7 @@ namespace optimalcontrol {
 
             toEigen(constraintIterator->get()->lambdaBuffer) = toEigen(lambda).topRows(constraintIterator->get()->lambdaBuffer.size());
 
-            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTState(time, state, control, lambda, hessian))) {
+            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTState(time, state, control, constraintIterator->get()->lambdaBuffer, hessian))) {
                 std::ostringstream errorMsg;
                 errorMsg << "Failed to evaluate "<< constraintIterator->get()->constraint->name() << std::endl;
                 reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTState", errorMsg.str().c_str());
@@ -671,7 +723,7 @@ namespace optimalcontrol {
 
             toEigen(constraintIterator->get()->lambdaBuffer) = toEigen(lambda).topRows(constraintIterator->get()->lambdaBuffer.size());
 
-            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTControl(time, state, control, lambda, hessian))) {
+            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTControl(time, state, control, constraintIterator->get()->lambdaBuffer, hessian))) {
                 std::ostringstream errorMsg;
                 errorMsg << "Failed to evaluate "<< constraintIterator->get()->constraint->name() << std::endl;
                 reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTControl", errorMsg.str().c_str());
@@ -733,7 +785,7 @@ namespace optimalcontrol {
 
             toEigen(constraintIterator->get()->lambdaBuffer) = toEigen(lambda).topRows(constraintIterator->get()->lambdaBuffer.size());
 
-            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTStateControl(time, state, control, lambda, hessian))) {
+            if (!(constraintIterator->get()->constraint->constraintSecondPartialDerivativeWRTStateControl(time, state, control, constraintIterator->get()->lambdaBuffer, hessian))) {
                 std::ostringstream errorMsg;
                 errorMsg << "Failed to evaluate "<< constraintIterator->get()->constraint->name() << std::endl;
                 reportError("ConstraintsGroup", "constraintSecondPartialDerivativeWRTStateControl", errorMsg.str().c_str());
@@ -757,6 +809,39 @@ namespace optimalcontrol {
             return true;
         }
 
+        bool ConstraintsGroup::constraintsSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            if (!(m_pimpl->stateHessianSparsityProvided)) {
+                return false;
+            }
+
+            stateSparsity = m_pimpl->groupStateHessianSparsity;
+
+            return true;
+        }
+
+        bool ConstraintsGroup::constraintsSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            if (!(m_pimpl->mixedHessianSparsityProvided)) {
+                return false;
+            }
+
+            stateControlSparsity = m_pimpl->groupMixedHessianSparsity;
+
+            return true;
+        }
+
+        bool ConstraintsGroup::constraintsSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            if (!(m_pimpl->controlHessianSparsityProvided)) {
+                return false;
+            }
+
+            controlSparsity = m_pimpl->groupControlHessianSparsity;
+
+            return true;
+        }
+
         bool ConstraintsGroup::isAnyTimeGroup()
         {
             if ((numberOfConstraints() == 1) && (m_pimpl->group.begin()->second.get()->timeRange == TimeRange::AnyTime())) {
diff --git a/src/optimalcontrol/src/Cost.cpp b/src/optimalcontrol/src/Cost.cpp
index 61784bb3ec3..be49111144a 100644
--- a/src/optimalcontrol/src/Cost.cpp
+++ b/src/optimalcontrol/src/Cost.cpp
@@ -32,7 +32,7 @@ namespace iDynTree {
             return m_costName;
         }
 
-        bool Cost::costFirstPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, VectorDynSize &partialDerivative)
+        bool Cost::costFirstPartialDerivativeWRTState(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, VectorDynSize &/*partialDerivative*/)
         {
             std::ostringstream errorMsg;
             errorMsg << "Method not implemented for cost "<< name() << std::endl;
@@ -40,7 +40,7 @@ namespace iDynTree {
             return false;
         }
 
-        bool Cost::costFirstPartialDerivativeWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, VectorDynSize &partialDerivative)
+        bool Cost::costFirstPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, VectorDynSize &/*partialDerivative*/)
         {
             std::ostringstream errorMsg;
             errorMsg << "Method not implemented for cost "<< name() << std::endl;
@@ -48,7 +48,7 @@ namespace iDynTree {
             return false;
         }
 
-        bool Cost::costSecondPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &partialDerivative)
+        bool Cost::costSecondPartialDerivativeWRTState(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, MatrixDynSize &/*partialDerivative*/)
         {
             std::ostringstream errorMsg;
             errorMsg << "Method not implemented for cost "<< name() << std::endl;
@@ -56,7 +56,7 @@ namespace iDynTree {
             return false;
         }
 
-        bool Cost::costSecondPartialDerivativeWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &partialDerivative)
+        bool Cost::costSecondPartialDerivativeWRTControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, MatrixDynSize &/*partialDerivative*/)
         {
             std::ostringstream errorMsg;
             errorMsg << "Method not implemented for cost "<< name() << std::endl;
@@ -64,7 +64,7 @@ namespace iDynTree {
             return false;
         }
 
-        bool Cost::costSecondPartialDerivativeWRTStateControl(double time, const VectorDynSize &state, const VectorDynSize &control, MatrixDynSize &partialDerivative)
+        bool Cost::costSecondPartialDerivativeWRTStateControl(double /*time*/, const VectorDynSize &/*state*/, const VectorDynSize &/*control*/, MatrixDynSize &/*partialDerivative*/)
         {
             std::ostringstream errorMsg;
             errorMsg << "Method not implemented for cost "<< name() << std::endl;
@@ -72,6 +72,21 @@ namespace iDynTree {
             return false;
         }
 
+        bool Cost::costSecondPartialDerivativeWRTStateSparsity(SparsityStructure &/*stateSparsity*/)
+        {
+            return false;
+        }
+
+        bool Cost::costSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &/*stateControlSparsity*/)
+        {
+            return false;
+        }
+
+        bool Cost::costSecondPartialDerivativeWRTControlSparsity(SparsityStructure &/*controlSparsity*/)
+        {
+            return false;
+        }
+
     }
 
 }
diff --git a/src/optimalcontrol/src/DynamicalSystem.cpp b/src/optimalcontrol/src/DynamicalSystem.cpp
index 5fd9e2b717f..a49fac8d3dc 100644
--- a/src/optimalcontrol/src/DynamicalSystem.cpp
+++ b/src/optimalcontrol/src/DynamicalSystem.cpp
@@ -115,6 +115,21 @@ namespace iDynTree {
             return false;
         }
 
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTStateSparsity(iDynTree::optimalcontrol::SparsityStructure &/*stateSparsity*/)
+        {
+            return false;
+        }
+
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &/*stateControlSparsity*/)
+        {
+            return false;
+        }
+
+        bool DynamicalSystem::dynamicsSecondPartialDerivativeWRTControlSparsity(SparsityStructure &/*controlSparsity*/)
+        {
+            return false;
+        }
+
 
 
     }
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index 4a6f3fec05c..c7d8981d81b 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -73,14 +73,36 @@ namespace iDynTree {
                         m_stateJacobianSparsity[1].nonZeroElementColumns.push_back(i);
                     }
 
-                    m_hasStateSparsity = true;
+                    m_hasStateJacobianSparsity = true;
                 }
 
                 if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0])) {
                     m_controlJacobianSparsity[1].nonZeroElementRows.clear();
                     m_controlJacobianSparsity[1].nonZeroElementColumns.clear();
 
-                    m_hasControlSparsity = true;
+                    m_hasControlJacobianSparsity = true;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateSparsity(m_stateHessianSparsity[CollocationHessianIndex(0, 0)])) {
+                    m_stateHessianSparsity[CollocationHessianIndex(0, 1)].clear();
+                    m_stateHessianSparsity[CollocationHessianIndex(1, 1)].clear();
+
+                    m_hasStateHessianSparsity = true;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControlSparsity(m_stateControlHessianSparsity[CollocationHessianIndex(0, 0)])) {
+                    m_stateControlHessianSparsity[CollocationHessianIndex(0, 1)].clear();
+                    m_stateControlHessianSparsity[CollocationHessianIndex(1, 0)].clear();
+                    m_stateControlHessianSparsity[CollocationHessianIndex(1, 1)].clear();
+
+                    m_hasStateControlHessianSparsity = true;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControlSparsity(m_controlHessianSparsity[CollocationHessianIndex(0, 0)])) {
+                    m_controlHessianSparsity[CollocationHessianIndex(0, 1)].clear();
+                    m_controlHessianSparsity[CollocationHessianIndex(1, 1)].clear();
+
+                    m_hasControlHessianSparsity = true;
                 }
 
                 return true;
@@ -259,7 +281,7 @@ namespace iDynTree {
 
             bool ForwardEuler::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &stateJacobianSparsity)
             {
-                if (!m_hasStateSparsity) {
+                if (!m_hasStateJacobianSparsity) {
                     return false;
                 }
 
@@ -269,7 +291,7 @@ namespace iDynTree {
 
             bool ForwardEuler::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &controlJacobianSparsity)
             {
-                if (!m_hasControlSparsity) {
+                if (!m_hasControlJacobianSparsity) {
                     return false;
                 }
 
@@ -352,6 +374,36 @@ namespace iDynTree {
 
                 return true;
             }
+
+            bool ForwardEuler::getCollocationConstraintSecondDerivativeWRTStateSparsity(CollocationHessianSparsityMap &stateDerivativeSparsity)
+            {
+                if (!m_hasStateHessianSparsity) {
+                    return false;
+                }
+
+                stateDerivativeSparsity = m_stateHessianSparsity;
+                return true;
+            }
+
+            bool ForwardEuler::getCollocationConstraintSecondDerivativeWRTControlSparsity(CollocationHessianSparsityMap &controlDerivativeSparsity)
+            {
+                if (!m_hasControlHessianSparsity) {
+                    return false;
+                }
+
+                controlDerivativeSparsity = m_controlHessianSparsity;
+                return true;
+            }
+
+            bool ForwardEuler::getCollocationConstraintSecondDerivativeWRTStateControlSparsity(CollocationHessianSparsityMap &stateControlDerivativeSparsity)
+            {
+                if (!m_hasStateControlHessianSparsity) {
+                    return false;
+                }
+
+                stateControlDerivativeSparsity = m_stateControlHessianSparsity;
+                return true;
+            }
         }
     }
 }
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index f08abdcff4d..0a2b875e3bc 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -73,21 +73,43 @@ namespace iDynTree {
                     }
                     m_stateJacobianSparsity[1] = m_stateJacobianSparsity[0];
 
-                    m_hasStateSparsity = true;
+                    m_hasStateJacobianSparsity = true;
                 }
 
                 if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0])) {
 
                     m_controlJacobianSparsity[1] = m_controlJacobianSparsity[0];
 
-                    m_hasControlSparsity = true;
+                    m_hasControlJacobianSparsity = true;
 
                 }
 
+                if (m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateSparsity(m_stateHessianSparsity[CollocationHessianIndex(0, 0)])) {
+                    m_stateHessianSparsity[CollocationHessianIndex(0, 1)].clear();
+                    m_stateHessianSparsity[CollocationHessianIndex(1, 1)] = m_stateHessianSparsity[CollocationHessianIndex(0, 0)];
+
+                    m_hasStateHessianSparsity = true;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTStateControlSparsity(m_stateControlHessianSparsity[CollocationHessianIndex(0, 0)])) {
+                    m_stateControlHessianSparsity[CollocationHessianIndex(0, 1)].clear();
+                    m_stateControlHessianSparsity[CollocationHessianIndex(1, 0)].clear();
+                    m_stateControlHessianSparsity[CollocationHessianIndex(1, 1)] = m_stateControlHessianSparsity[CollocationHessianIndex(0, 0)];
+
+                    m_hasStateControlHessianSparsity = true;
+                }
+
+                if (m_dynamicalSystem_ptr->dynamicsSecondPartialDerivativeWRTControlSparsity(m_controlHessianSparsity[CollocationHessianIndex(0, 0)])) {
+                    m_controlHessianSparsity[CollocationHessianIndex(0, 1)].clear();
+                    m_controlHessianSparsity[CollocationHessianIndex(1, 1)] = m_controlHessianSparsity[CollocationHessianIndex(0, 0)];
+
+                    m_hasControlHessianSparsity = true;
+                }
+
                 return true;
             }
 
-            bool ImplicitTrapezoidal::oneStepIntegration(double t0, double dT, const iDynTree::VectorDynSize &x0, iDynTree::VectorDynSize &x)
+            bool ImplicitTrapezoidal::oneStepIntegration(double /*t0*/, double /*dT*/, const iDynTree::VectorDynSize &/*x0*/, iDynTree::VectorDynSize &/*x*/)
             {
                 reportError(m_info.name().c_str(), "oneStepIntegration", "The ImplicitTrapezoidal method has not been implemented to integrate a dynamical system yet.");
                 return false;
@@ -270,7 +292,7 @@ namespace iDynTree {
 
             bool ImplicitTrapezoidal::getCollocationConstraintJacobianStateSparsity(std::vector<SparsityStructure> &stateJacobianSparsity)
             {
-                if (!m_hasStateSparsity) {
+                if (!m_hasStateJacobianSparsity) {
                     return false;
                 }
 
@@ -280,7 +302,7 @@ namespace iDynTree {
 
             bool ImplicitTrapezoidal::getCollocationConstraintJacobianControlSparsity(std::vector<SparsityStructure> &controlJacobianSparsity)
             {
-                if (!m_hasControlSparsity) {
+                if (!m_hasControlJacobianSparsity) {
                     return false;
                 }
 
@@ -389,6 +411,36 @@ namespace iDynTree {
                 return true;
             }
 
+            bool ImplicitTrapezoidal::getCollocationConstraintSecondDerivativeWRTStateSparsity(CollocationHessianSparsityMap &stateDerivativeSparsity)
+            {
+                if (!m_hasStateHessianSparsity) {
+                    return false;
+                }
+
+                stateDerivativeSparsity = m_stateHessianSparsity;
+                return true;
+            }
+
+            bool ImplicitTrapezoidal::getCollocationConstraintSecondDerivativeWRTControlSparsity(CollocationHessianSparsityMap &controlDerivativeSparsity)
+            {
+                if (!m_hasControlHessianSparsity) {
+                    return false;
+                }
+
+                controlDerivativeSparsity = m_controlHessianSparsity;
+                return true;
+            }
+
+            bool ImplicitTrapezoidal::getCollocationConstraintSecondDerivativeWRTStateControlSparsity(CollocationHessianSparsityMap &stateControlDerivativeSparsity)
+            {
+                if (!m_hasStateControlHessianSparsity) {
+                    return false;
+                }
+
+                stateControlDerivativeSparsity = m_stateControlHessianSparsity;
+                return true;
+            }
+
         }
     }
 }
diff --git a/src/optimalcontrol/src/Integrator.cpp b/src/optimalcontrol/src/Integrator.cpp
index 4bc1c8ba781..3a639afb464 100644
--- a/src/optimalcontrol/src/Integrator.cpp
+++ b/src/optimalcontrol/src/Integrator.cpp
@@ -150,6 +150,21 @@ namespace iDynTree {
                 return false;
             }
 
+            bool Integrator::getCollocationConstraintSecondDerivativeWRTStateSparsity(CollocationHessianSparsityMap &/*stateDerivativeSparsity*/)
+            {
+                return false;
+            }
+
+            bool Integrator::getCollocationConstraintSecondDerivativeWRTControlSparsity(CollocationHessianSparsityMap &/*controlDerivativeSparsity*/)
+            {
+                return false;
+            }
+
+            bool Integrator::getCollocationConstraintSecondDerivativeWRTStateControlSparsity(CollocationHessianSparsityMap &/*stateControlDerivativeSparsity*/)
+            {
+                return false;
+            }
+
             const IntegratorInfo &Integrator::info() const
             {
                 return m_info;
@@ -192,6 +207,16 @@ namespace iDynTree {
                 return (m_first < rhs.m_first) || ((m_first == rhs.m_first) && (m_second < rhs.m_second));
             }
 
+            size_t CollocationHessianIndex::first() const
+            {
+                return m_first;
+            }
+
+            size_t CollocationHessianIndex::second() const
+            {
+                return m_second;
+            }
+
         }
 
     }
diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 8679abb5b5e..9296fcd6fb0 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -33,6 +33,7 @@ namespace iDynTree {
         protected:
             VectorDynSize m_selected;
             MatrixDynSize m_selectionMatrix;
+            SparsityStructure m_correspondingHessianSparsity;
         public:
             Selector(unsigned int selectedSize) : m_selected(selectedSize) { }
 
@@ -44,6 +45,14 @@ namespace iDynTree {
                 return m_selectionMatrix;
             }
 
+            const SparsityStructure& sparsity() {
+                return m_correspondingHessianSparsity;
+            }
+
+            void setSparsity(const SparsityStructure& sparsity) {
+                m_correspondingHessianSparsity = sparsity;
+            }
+
             unsigned int size() const {
                 return m_selected.size();
             }
@@ -60,6 +69,7 @@ namespace iDynTree {
             {
                 m_selectionMatrix.resize(static_cast<unsigned int>(selectedRange.size), totalSize);
                 toEigen(m_selectionMatrix).block(0, selectedRange.offset, selectedRange.size, selectedRange.size).setIdentity();
+                m_correspondingHessianSparsity.addDenseBlock(selectedRange, selectedRange);
             }
 
             ~IndexSelector() override;
@@ -69,6 +79,7 @@ namespace iDynTree {
                 m_selected = make_span(fullVector).subspan(m_selectedIndex.offset, m_selectedIndex.size);
                 return m_selected;
             }
+
         };
         IndexSelector::~IndexSelector(){}
 
@@ -78,6 +89,7 @@ namespace iDynTree {
                 : Selector(selectorMatrix.rows())
             {
                 m_selectionMatrix = selectorMatrix;
+                m_correspondingHessianSparsity.addDenseBlock(0, 0, selectorMatrix.cols(), selectorMatrix.cols()); //dense
             }
 
             ~MatrixSelector() override;
@@ -91,11 +103,13 @@ namespace iDynTree {
         MatrixSelector::~MatrixSelector(){}
 
         //
-        // End of implementation of TimeVaryingGradient
+        // End of implementation of selectors
         //
 
         class CostAttributes {
             bool m_valid = false;
+            SparsityStructure m_emptySparsity; //dummy sparsity in case this cost is not initialized
+
         public:
             std::shared_ptr<TimeVaryingVector> desiredTrajectory;
             iDynTree::MatrixDynSize gradientSubMatrix, hessianMatrix;
@@ -163,7 +177,7 @@ namespace iDynTree {
                 }
 
                 iDynTree::toEigen(this->weightMatrix) = iDynTree::toEigen(weights).asDiagonal();
-                toEigen(this->gradientSubMatrix) = 0.5 * toEigen(this->selector->asSelectorMatrix()).transpose() * (toEigen(this->weightMatrix) + toEigen(this->weightMatrix).transpose());
+                toEigen(this->gradientSubMatrix) = toEigen(this->selector->asSelectorMatrix()).transpose() * toEigen(this->weightMatrix);
                 toEigen(this->hessianMatrix) = toEigen(this->gradientSubMatrix) * toEigen(this->selector->asSelectorMatrix());
 
                 return true;
@@ -287,6 +301,13 @@ namespace iDynTree {
 
             }
 
+            const SparsityStructure& getHessianSparsity() {
+                if (!isValid()) {
+                    return m_emptySparsity;
+                }
+                return selector->sparsity();
+            }
+
         };
 
         //
@@ -554,6 +575,24 @@ namespace iDynTree {
             return true;
         }
 
+        bool L2NormCost::costSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            stateSparsity = m_pimpl->stateCost.getHessianSparsity();
+            return true;
+        }
+
+        bool L2NormCost::costSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            stateControlSparsity.clear();
+            return true;
+        }
+
+        bool L2NormCost::costSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            controlSparsity = m_pimpl->stateCost.getHessianSparsity();
+            return true;
+        }
+
     }
 }
 
diff --git a/src/optimalcontrol/src/LinearConstraint.cpp b/src/optimalcontrol/src/LinearConstraint.cpp
index 1c7b05307d1..b732d1aaff2 100644
--- a/src/optimalcontrol/src/LinearConstraint.cpp
+++ b/src/optimalcontrol/src/LinearConstraint.cpp
@@ -277,5 +277,23 @@ namespace iDynTree {
             return true;
         }
 
+        bool LinearConstraint::constraintSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            stateSparsity.clear();
+            return true;
+        }
+
+        bool LinearConstraint::constraintSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            stateControlSparsity.clear();
+            return true;
+        }
+
+        bool LinearConstraint::constraintSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            controlSparsity.clear();
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/LinearCost.cpp b/src/optimalcontrol/src/LinearCost.cpp
index 64a7d97c4b2..cdc1e002e55 100644
--- a/src/optimalcontrol/src/LinearCost.cpp
+++ b/src/optimalcontrol/src/LinearCost.cpp
@@ -22,7 +22,11 @@ namespace iDynTree {
 
         LinearCost::LinearCost(const std::string &costName)
             : QuadraticLikeCost(costName)
-        { }
+        {
+            m_hasSecondPartialDerivativeWRTStateSparsity = true;
+            m_hasSecondPartialDerivativeWRTControlSparsity = true;
+            m_hasSecondPartialDerivativeWRTStateControlSparsity = true;
+        }
 
         LinearCost::~LinearCost()
         { }
diff --git a/src/optimalcontrol/src/LinearSystem.cpp b/src/optimalcontrol/src/LinearSystem.cpp
index 3a4cf1c0b8d..98678403eb3 100644
--- a/src/optimalcontrol/src/LinearSystem.cpp
+++ b/src/optimalcontrol/src/LinearSystem.cpp
@@ -264,5 +264,23 @@ namespace iDynTree {
             return true;
         }
 
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            stateSparsity.clear();
+            return true;
+        }
+
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            stateControlSparsity.clear();
+            return true;
+        }
+
+        bool LinearSystem::dynamicsSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            controlSparsity.clear();
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 855bbb35e40..11bee173d28 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -88,6 +88,7 @@ namespace iDynTree {
         //MARK: Transcription implementation
         class HessianBlocksMap {
             std::map<CollocationHessianIndex, bool> m_map;
+            std::map<CollocationHessianIndex, SparsityStructure> m_sparsity;
 
         public:
 
@@ -104,8 +105,28 @@ namespace iDynTree {
                 return it->second;
             }
 
+            SparsityStructure& blockSparsity(size_t row, size_t col) {
+                CollocationHessianIndex indices(row, col);
+                std::map<CollocationHessianIndex, SparsityStructure>::iterator it = m_sparsity.find(indices);
+
+                if (it == m_sparsity.end()) {
+                    std::pair<std::map<CollocationHessianIndex, SparsityStructure>::iterator, bool> result = m_sparsity.insert(std::make_pair(indices, SparsityStructure()));
+                    it = result.first;
+                }
+                return it->second;
+            }
+
+            void getFullSparsity(SparsityStructure& sparsity) {
+                sparsity.clear();
+
+                for (auto& block : m_sparsity) {
+                    sparsity.addBlock(block.first.first(), block.first.second(), block.second);
+                }
+            }
+
             void clear() {
                 m_map.clear();
+                m_sparsity.clear();
             }
 
             void reset() {
@@ -128,11 +149,11 @@ namespace iDynTree {
             std::vector<MeshPoint>::iterator m_meshPointsEnd;
             double m_minStepSize, m_maxStepSize, m_controlPeriod;
             size_t m_nx, m_nu, m_numberOfVariables, m_constraintsPerInstant, m_numberOfConstraints;
-            std::vector<size_t> m_jacobianNZRows, m_jacobianNZCols, m_hessianNZRows, m_hessianNZCols;
-            SparsityStructure m_ocStateSparsity, m_ocControlSparsity;
-            std::vector<SparsityStructure> m_collocationStateNZ, m_collocationControlNZ;
-            SparsityStructure m_mergedCollocationControlNZ;
-            size_t m_jacobianNonZeros, m_hessianNonZeros;
+            std::vector<size_t> m_jacobianNZRows, m_jacobianNZCols/*, m_hessianNZRows, m_hessianNZCols*/;
+            SparsityStructure m_ocStateJacobianSparsity, m_ocControlJacobianSparsity;
+            std::vector<SparsityStructure> m_collocationStateJacobianNZ, m_collocationControlJacobianNZ;
+            SparsityStructure m_mergedCollocationControlJacobianNZ;
+            size_t m_jacobianNonZeros/*, m_hessianNonZeros*/;
             double m_plusInfinity, m_minusInfinity;
             VectorDynSize m_constraintsLowerBound, m_constraintsUpperBound;
             VectorDynSize m_constraintsBuffer, m_stateBuffer, m_controlBuffer, m_variablesBuffer, m_guessBuffer, m_costStateGradientBuffer, m_costControlGradientBuffer;
@@ -143,11 +164,17 @@ namespace iDynTree {
             VectorDynSize m_lambdaConstraints, m_lambdaCollocation;
             MatrixDynSize m_stateHessianBuffer, m_controlHessianBuffer, m_stateControlHessianBuffer;
             CollocationHessianMap m_stateCollocationHessians, m_controlCollocationHessians, m_stateControCollocationlHessians;
-            HessianBlocksMap m_hessianBlocksSet;
+            HessianBlocksMap m_hessianBlocks;
             VectorDynSize m_solution;
             std::shared_ptr<TimeVaryingVector> m_stateGuesses, m_controlGuesses;
             bool m_solved;
 
+            SparsityStructure m_constraintsStateHessianSparsity, m_constraintsControlHessianSparsity, m_constraintsMixedHessianSparsity;
+            SparsityStructure m_costsStateHessianSparsity, m_costsControlHessianSparsity, m_costsMixedHessianSparsity;
+            CollocationHessianSparsityMap m_systemStateHessianSparsity, m_systemControlHessianSparsity, m_systemMixedHessianSparsity;
+            SparsityStructure m_fullHessianSparsity;
+
+
             friend class MultipleShootingSolver;
 
             void resetMeshPoints(){
@@ -201,7 +228,7 @@ namespace iDynTree {
 
             void resetNonZerosCount(){
                 m_jacobianNonZeros = 0;
-                m_hessianNonZeros = 0;
+                m_hessianBlocks.clear();
             }
 
             void addNonZero(std::vector<size_t>& input, size_t position, size_t toBeAdded){
@@ -240,54 +267,54 @@ namespace iDynTree {
             }
 
             void addHessianBlock(size_t initRow, size_t rows, size_t initCol, size_t cols){
-                if (!m_hessianBlocksSet(initRow, initCol)) {
-                    for (size_t i = 0; i < rows; ++i){
-                        for (size_t j = 0; j < cols; ++j){
-                            addNonZero(m_hessianNZRows, m_hessianNonZeros, initRow + i);
-                            addNonZero(m_hessianNZCols, m_hessianNonZeros, initCol + j);
-                            m_hessianNonZeros++;
-                        }
-                    }
 
-                    m_hessianBlocksSet(initRow, initCol) = true;
-                }
+                m_hessianBlocks.blockSparsity(initRow, initCol).addDenseBlock(0, 0, rows, cols);
 
                 //add also the transpose
 
-                if (!m_hessianBlocksSet(initCol, initRow)) {
-                    for (size_t i = 0; i < cols; ++i){
-                        for (size_t j = 0; j < rows; ++j){
-                            addNonZero(m_hessianNZRows, m_hessianNonZeros, initCol + i);
-                            addNonZero(m_hessianNZCols, m_hessianNonZeros, initRow + j);
-                            m_hessianNonZeros++;
-                        }
-                    }
+                m_hessianBlocks.blockSparsity(initCol, initRow).addDenseBlock(0, 0, cols, rows);
 
-                    m_hessianBlocksSet(initCol, initRow) = true;
-                }
+            }
+
+            void addHessianBlock(size_t initRow, size_t initCol, const SparsityStructure& sparsity){
+
+                m_hessianBlocks.blockSparsity(initRow, initCol).merge(sparsity);
 
+                //add also the transpose
+
+                SparsityStructure& transposeSparsity = m_hessianBlocks.blockSparsity(initCol, initRow);
+
+                for (size_t i = 0; i < sparsity.size(); ++i){
+                    transposeSparsity.addNonZeroIfNotPresent(sparsity.nonZeroElementColumns[i], sparsity.nonZeroElementRows[i]);
+                }
             }
 
             void setHessianBlock(MatrixDynSize& hessian, const MatrixDynSize& block, size_t startRow, size_t startCol) {
-                if (m_hessianBlocksSet(startRow, startCol)) {
+                if (m_hessianBlocks(startRow, startCol)) {
                     toEigen(hessian).block(static_cast<Eigen::Index>(startRow), static_cast<unsigned int>(startCol), block.rows(), block.cols()) += toEigen(block);
                 } else {
                     toEigen(hessian).block(static_cast<Eigen::Index>(startRow), static_cast<unsigned int>(startCol), block.rows(), block.cols()) = toEigen(block);
-                    m_hessianBlocksSet(startRow, startCol) = true;
+                    m_hessianBlocks(startRow, startCol) = true;
                 }
             }
 
             void setHessianBlockAndItsTranspose(MatrixDynSize& hessian, const MatrixDynSize& block, size_t startRow, size_t startCol) {
                 setHessianBlock(hessian, block, startRow, startCol);
 
-                if (m_hessianBlocksSet(startCol, startRow)) {
+                if (m_hessianBlocks(startCol, startRow)) {
                     toEigen(hessian).block(static_cast<Eigen::Index>(startCol), static_cast<unsigned int>(startRow), block.cols(), block.rows()) += toEigen(block).transpose();
                 } else {
                     toEigen(hessian).block(static_cast<Eigen::Index>(startCol), static_cast<unsigned int>(startRow), block.cols(), block.rows()) = toEigen(block).transpose();
-                    m_hessianBlocksSet(startCol, startRow) = true;
+                    m_hessianBlocks(startCol, startRow) = true;
                 }
+            }
 
-
+            void addHessianSparsityBlock(bool isSparse, size_t row, size_t numberOfRows, size_t col, size_t numberOfCols, const SparsityStructure& sparsity) {
+                if (isSparse) {
+                    this->addHessianBlock(row, col, sparsity);
+                } else {
+                    this->addHessianBlock(row, numberOfRows, col, numberOfCols);
+                }
             }
 
             void mergeSparsityVectors(const std::vector<SparsityStructure>& original, SparsityStructure& merged) {
@@ -1101,25 +1128,55 @@ namespace iDynTree {
                 Eigen::Map<Eigen::VectorXd> lowerBoundMap = toEigen(m_constraintsLowerBound);
                 Eigen::Map<Eigen::VectorXd> upperBoundMap = toEigen(m_constraintsUpperBound);
 
-                bool ocHasStateSparsisty = m_ocproblem->constraintJacobianWRTStateSparsity(m_ocStateSparsity);
-                if (!ocHasStateSparsisty) {
-                    reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve state sparsity of optimal control problem constraints. Assuming dense matrix.");
+                bool ocHasStateJacobianSparsisty = m_ocproblem->constraintsJacobianWRTStateSparsity(m_ocStateJacobianSparsity);
+                if (!ocHasStateJacobianSparsisty) {
+                    reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve state sparsity of optimal control problem constraints jacobian. Assuming dense matrix.");
                 }
 
-                bool ocHasControlSparsisty = m_ocproblem->constraintJacobianWRTControlSparsity(m_ocControlSparsity);
-                if (!ocHasControlSparsisty) {
-                    reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve control sparsity of optimal control problem constraints. Assuming dense matrix.");
+                bool ocHasControlJacobianSparsisty = m_ocproblem->constraintsJacobianWRTControlSparsity(m_ocControlJacobianSparsity);
+                if (!ocHasControlJacobianSparsisty) {
+                    reportWarning("MultipleShootingTranscription", "prepare", "Failed to retrieve control sparsity of optimal control problem constraints jacobian. Assuming dense matrix.");
                 }
 
-                bool systemHasStateSparsity = m_integrator->getCollocationConstraintJacobianStateSparsity(m_collocationStateNZ);
-                bool systemHasControlSparsity = m_integrator->getCollocationConstraintJacobianControlSparsity(m_collocationControlNZ);
+                bool systemHasStateJacobianSparsity = m_integrator->getCollocationConstraintJacobianStateSparsity(m_collocationStateJacobianNZ);
 
-                if (systemHasControlSparsity) {
-                    mergeSparsityVectors(m_collocationControlNZ, m_mergedCollocationControlNZ);
+                bool systemHasControlJacobianSparsity = m_integrator->getCollocationConstraintJacobianControlSparsity(m_collocationControlJacobianNZ);
+
+                if (systemHasControlJacobianSparsity) {
+                    mergeSparsityVectors(m_collocationControlJacobianNZ, m_mergedCollocationControlJacobianNZ);
                 }
 
+                auto printError = [](bool ok, const std::string& hessianName, const std::string& variableType) {
+                    if (!ok) {
+                        reportWarning("MultipleShootingTranscription", "prepare", ("Failed to retrieve " + variableType+ " sparsity of optimal control problem " +
+                                                                                   hessianName + " hessian. Assuming dense matrix.").c_str());
+                    }
+                };
+
+                bool constraintsHaveStateHessianSparsity = m_ocproblem->constraintsSecondPartialDerivativeWRTStateSparsity(m_constraintsStateHessianSparsity);
+                printError(constraintsHaveStateHessianSparsity, "constraints", "state");
+
+                bool constraintsHaveControlHessianSparsity = m_ocproblem->constraintsSecondPartialDerivativeWRTControlSparsity(m_constraintsControlHessianSparsity);
+                printError(constraintsHaveControlHessianSparsity, "constraints", "control");
+
+                bool constraintsHaveMixedHessianSparsity = m_ocproblem->constraintsSecondPartialDerivativeWRTStateControlSparsity(m_constraintsMixedHessianSparsity);
+                printError(constraintsHaveMixedHessianSparsity, "constraints", "state/control");
+
+
+                bool costsHaveStateHessianSparsity = m_ocproblem->costsSecondPartialDerivativeWRTStateSparsity(m_costsStateHessianSparsity);
+                printError(costsHaveStateHessianSparsity, "costs", "state");
+
+                bool costsHaveControlHessianSparsity = m_ocproblem->costsSecondPartialDerivativeWRTControlSparsity(m_costsControlHessianSparsity);
+                printError(costsHaveControlHessianSparsity, "costs", "control");
+
+                bool costsHaveMixedHessianSparsity = m_ocproblem->costsSecondPartialDerivativeWRTStateControlSparsity(m_costsMixedHessianSparsity);
+                printError(costsHaveMixedHessianSparsity, "costs", "state/control");
+
+                bool systemHasStateHessianSparsity = m_integrator->getCollocationConstraintSecondDerivativeWRTStateSparsity(m_systemStateHessianSparsity);
+                bool systemHasControlHessianSparsity = m_integrator->getCollocationConstraintSecondDerivativeWRTControlSparsity(m_systemControlHessianSparsity);
+                bool systemHasMixedHessianSparsity = m_integrator->getCollocationConstraintSecondDerivativeWRTStateControlSparsity(m_systemMixedHessianSparsity);
+
                 resetNonZerosCount();
-                m_hessianBlocksSet.clear();
 
                 std::vector<MeshPoint>::iterator mesh = m_meshPoints.begin(), previousControlMesh = mesh;
                 size_t index = 0, constraintIndex = 0;
@@ -1158,19 +1215,21 @@ namespace iDynTree {
                         }
 
                         //Saving the jacobian structure due to the constraints (state should not be constrained here)
-                        if (ocHasControlSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlSparsity);
+                        if (ocHasControlJacobianSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlJacobianSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         }
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        if (!m_info.costIsLinear() || m_info.hasNonLinearConstraints()) {
-                            addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-                            addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
-
-                            addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
+                        if (!m_info.costIsLinear()) {
+                            addHessianSparsityBlock(costsHaveStateHessianSparsity, mesh->stateIndex, nx, mesh->stateIndex, nx, m_costsStateHessianSparsity);
+                            addHessianSparsityBlock(costsHaveMixedHessianSparsity, mesh->stateIndex, nx, mesh->controlIndex, nu, m_costsMixedHessianSparsity);
+                            addHessianSparsityBlock(costsHaveControlHessianSparsity, mesh->controlIndex, nu, mesh->controlIndex, nu, m_costsControlHessianSparsity);
+                        }
+                        if (m_info.hasNonLinearConstraints()){
+                            addHessianSparsityBlock(constraintsHaveControlHessianSparsity, mesh->controlIndex, nu, mesh->controlIndex, nu, m_constraintsControlHessianSparsity);
                         }
 
                     } else if (mesh->type == MeshPointType::Control) {
@@ -1186,17 +1245,17 @@ namespace iDynTree {
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
 
                         //Saving the jacobian structure due to the dynamical constraints
-                        if (systemHasControlSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_collocationControlNZ[1]);
-                            addJacobianBlock(constraintIndex, mesh->previousControlIndex, m_collocationControlNZ[0]);
+                        if (systemHasControlJacobianSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_collocationControlJacobianNZ[1]);
+                            addJacobianBlock(constraintIndex, mesh->previousControlIndex, m_collocationControlJacobianNZ[0]);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
                             addJacobianBlock(constraintIndex, nx, mesh->previousControlIndex, nu);
                         }
 
-                        if (systemHasStateSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1]);
-                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0]);
+                        if (systemHasStateJacobianSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateJacobianNZ[1]);
+                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateJacobianNZ[0]);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
                             addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
@@ -1228,42 +1287,54 @@ namespace iDynTree {
                         }
 
                         //Saving the jacobian structure due to the constraints
-                        if (ocHasStateSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateSparsity);
+                        if (ocHasStateJacobianSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateJacobianSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         }
-                        if (ocHasControlSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlSparsity);
+                        if (ocHasControlJacobianSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlJacobianSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         }
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        if (!m_info.costIsLinear() || m_info.hasNonLinearConstraints()) {
-                            addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-
-                            addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
-
-                            addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-
+                        if (!m_info.costIsLinear()) {
+                            addHessianSparsityBlock(costsHaveStateHessianSparsity, mesh->stateIndex, nx, mesh->stateIndex, nx, m_costsStateHessianSparsity);
+                            addHessianSparsityBlock(costsHaveMixedHessianSparsity, mesh->stateIndex, nx, mesh->controlIndex, nu, m_costsMixedHessianSparsity);
+                            addHessianSparsityBlock(costsHaveControlHessianSparsity, mesh->controlIndex, nu, mesh->controlIndex, nu, m_costsControlHessianSparsity);
+                        }
+                        if (m_info.hasNonLinearConstraints()){
+                            addHessianSparsityBlock(constraintsHaveStateHessianSparsity, mesh->stateIndex, nx, mesh->stateIndex, nx, m_constraintsStateHessianSparsity);
+                            addHessianSparsityBlock(constraintsHaveMixedHessianSparsity, mesh->stateIndex, nx, mesh->controlIndex, nu, m_constraintsMixedHessianSparsity);
+                            addHessianSparsityBlock(constraintsHaveControlHessianSparsity, mesh->controlIndex, nu, mesh->controlIndex, nu, m_constraintsControlHessianSparsity);
                         }
 
-                        if (!(m_ocproblem->systemIsLinear())) { //if the system is not linear, automatically the above if is true
-
-                            addHessianBlock(mesh->previousControlIndex, nu, mesh->previousControlIndex, nu);
-                            addHessianBlock((mesh-1)->stateIndex, nx, (mesh-1)->stateIndex, nx);
-
-                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
-
-                            addHessianBlock(mesh->stateIndex, nx, mesh->previousControlIndex, nu);
-
-                            addHessianBlock(mesh->controlIndex, nu, mesh->previousControlIndex, nu);
-
-                            addHessianBlock(mesh->controlIndex, nu, (mesh - 1)->stateIndex, nx);
-
-                            addHessianBlock(mesh->previousControlIndex, nu, (mesh - 1)->stateIndex, nx);
+                        if (!(m_ocproblem->systemIsLinear())) {
+
+                            addHessianSparsityBlock(systemHasStateHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    (mesh-1)->stateIndex, nx, m_systemStateHessianSparsity[CollocationHessianIndex(0,0)]);
+                            addHessianSparsityBlock(systemHasStateHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    mesh->stateIndex, nx, m_systemStateHessianSparsity[CollocationHessianIndex(0,1)]);
+                            addHessianSparsityBlock(systemHasStateHessianSparsity, mesh->stateIndex, nx,
+                                                    mesh->stateIndex, nx, m_systemStateHessianSparsity[CollocationHessianIndex(1,1)]);
+
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    mesh->previousControlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(0,0)]);
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    mesh->controlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(0,1)]);
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, mesh->stateIndex, nx,
+                                                    mesh->previousControlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(1,0)]);
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, mesh->stateIndex, nx,
+                                                    mesh->controlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(1,1)]);
+
+                            addHessianSparsityBlock(systemHasControlHessianSparsity, mesh->previousControlIndex, nu,
+                                                    mesh->previousControlIndex, nu, m_systemControlHessianSparsity[CollocationHessianIndex(0,0)]);
+                            addHessianSparsityBlock(systemHasControlHessianSparsity, mesh->previousControlIndex, nu,
+                                                    mesh->controlIndex, nu, m_systemControlHessianSparsity[CollocationHessianIndex(0,1)]);
+                            addHessianSparsityBlock(systemHasControlHessianSparsity, mesh->controlIndex, nu,
+                                                    mesh->controlIndex, nu, m_systemControlHessianSparsity[CollocationHessianIndex(1,1)]);
                         }
 
 
@@ -1278,15 +1349,15 @@ namespace iDynTree {
                         upperBoundMap.segment(static_cast<Eigen::Index>(constraintIndex), static_cast<Eigen::Index>(nx)).setZero();
 
                         //Saving the jacobian structure due to the dynamical constraints
-                        if (systemHasControlSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_mergedCollocationControlNZ);
+                        if (systemHasControlJacobianSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_mergedCollocationControlJacobianNZ);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->controlIndex, nu);
                         }
 
-                        if (systemHasStateSparsity) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateNZ[1]);
-                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateNZ[0]);
+                        if (systemHasStateJacobianSparsity) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_collocationStateJacobianNZ[1]);
+                            addJacobianBlock(constraintIndex, (mesh - 1)->stateIndex, m_collocationStateJacobianNZ[0]);
                         } else {
                             addJacobianBlock(constraintIndex, nx, mesh->stateIndex, nx);
                             addJacobianBlock(constraintIndex, nx, (mesh - 1)->stateIndex, nx);
@@ -1316,37 +1387,54 @@ namespace iDynTree {
                             }
                         }
 
-                        if (ocHasStateSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateSparsity);
+                        if (ocHasStateJacobianSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->stateIndex, m_ocStateJacobianSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->stateIndex, nx);
                         }
-                        if (ocHasControlSparsisty) {
-                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlSparsity);
+                        if (ocHasControlJacobianSparsisty) {
+                            addJacobianBlock(constraintIndex, mesh->controlIndex, m_ocControlJacobianSparsity);
                         } else {
                             addJacobianBlock(constraintIndex, nc, mesh->controlIndex, nu);
                         }
                         constraintIndex += nc;
 
                         //Saving the hessian structure
-                        if (!m_info.costIsLinear() || m_info.hasNonLinearConstraints()) {
-                            addHessianBlock(mesh->controlIndex, nu, mesh->controlIndex, nu); //assume that a cost/constraint depends on the square of u
-
-                            addHessianBlock(mesh->stateIndex, nx, mesh->stateIndex, nx); //assume that a cost/constraint depends on the square of x
-
-                            addHessianBlock(mesh->controlIndex, nu, mesh->stateIndex, nx); //assume that a cost/constraint depends on the product of x-u
-
+                        if (!m_info.costIsLinear()) {
+                            addHessianSparsityBlock(costsHaveStateHessianSparsity, mesh->stateIndex, nx, mesh->stateIndex, nx, m_costsStateHessianSparsity);
+                            addHessianSparsityBlock(costsHaveMixedHessianSparsity, mesh->stateIndex, nx, mesh->controlIndex, nu, m_costsMixedHessianSparsity);
+//                            addHessianSparsityBlock(costsHaveControlHessianSparsity, mesh->controlIndex, nu, mesh->controlIndex, nu, m_costsControlHessianSparsity); // This was already inserted before
+                        }
+                        if (m_info.hasNonLinearConstraints()){
+                            addHessianSparsityBlock(constraintsHaveStateHessianSparsity, mesh->stateIndex, nx, mesh->stateIndex, nx, m_constraintsStateHessianSparsity);
+                            addHessianSparsityBlock(constraintsHaveMixedHessianSparsity, mesh->stateIndex, nx, mesh->controlIndex, nu, m_constraintsMixedHessianSparsity);
+//                            addHessianSparsityBlock(constraintsHaveControlHessianSparsity, mesh->controlIndex, nu, mesh->controlIndex, nu, m_constraintsControlHessianSparsity); // This was already inserted before
                         }
 
-                        if (!(m_ocproblem->systemIsLinear())) { //if the system is not linear, automatically the above if is true
-
-                            addHessianBlock((mesh-1)->stateIndex, nx, (mesh-1)->stateIndex, nx);
-
-                            addHessianBlock(mesh->stateIndex, nx, (mesh - 1)->stateIndex, nx);
-
-                            addHessianBlock(mesh->controlIndex, nu, (mesh - 1)->stateIndex, nx);
-
-                            //some blocks are missing since the previous control index and controlIndex coincide
+                        if (!(m_ocproblem->systemIsLinear())) {
+
+                            addHessianSparsityBlock(systemHasStateHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    (mesh-1)->stateIndex, nx, m_systemStateHessianSparsity[CollocationHessianIndex(0,0)]);
+                            addHessianSparsityBlock(systemHasStateHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    mesh->stateIndex, nx, m_systemStateHessianSparsity[CollocationHessianIndex(0,1)]);
+                            addHessianSparsityBlock(systemHasStateHessianSparsity, mesh->stateIndex, nx,
+                                                    mesh->stateIndex, nx, m_systemStateHessianSparsity[CollocationHessianIndex(1,1)]);
+
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    mesh->previousControlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(0,0)]);
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, (mesh-1)->stateIndex, nx,
+                                                    mesh->controlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(0,1)]);
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, mesh->stateIndex, nx,
+                                                    mesh->previousControlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(1,0)]);
+                            addHessianSparsityBlock(systemHasMixedHessianSparsity, mesh->stateIndex, nx,
+                                                    mesh->controlIndex, nu, m_systemMixedHessianSparsity[CollocationHessianIndex(1,1)]);
+
+//                            addHessianSparsityBlock(systemHasControlHessianSparsity, mesh->previousControlIndex, nu,
+//                                                    mesh->previousControlIndex, nu, m_systemControlHessianSparsity[CollocationHessianIndex(0,0)]); // This was already inserted before
+//                            addHessianSparsityBlock(systemHasControlHessianSparsity, mesh->previousControlIndex, nu,
+//                                                    mesh->controlIndex, nu, m_systemControlHessianSparsity[CollocationHessianIndex(0,1)]); // This was already inserted before
+//                            addHessianSparsityBlock(systemHasControlHessianSparsity, mesh->controlIndex, nu,
+//                                                    mesh->controlIndex, nu, m_systemControlHessianSparsity[CollocationHessianIndex(1,1)]); // This was already inserted before
                         }
 
                     }
@@ -1355,6 +1443,8 @@ namespace iDynTree {
                 assert(index == m_numberOfVariables);
                 assert(constraintIndex == m_numberOfConstraints);
 
+                m_hessianBlocks.getFullSparsity(m_fullHessianSparsity);
+
                 m_prepared = true;
                 return true;
             }
@@ -1539,18 +1629,8 @@ namespace iDynTree {
                     return false;
                 }
 
-                if (nonZeroElementRows.size() != m_hessianNonZeros) {
-                    nonZeroElementRows.resize(static_cast<unsigned int>(m_hessianNonZeros));
-                }
-
-                if (nonZeroElementColumns.size() != m_hessianNonZeros) {
-                    nonZeroElementColumns.resize(static_cast<unsigned int>(m_hessianNonZeros));
-                }
-
-                for (unsigned int i = 0; i < m_hessianNonZeros; ++i){
-                    nonZeroElementRows[i] = m_hessianNZRows[i];
-                    nonZeroElementColumns[i] = m_hessianNZCols[i];
-                }
+                nonZeroElementRows = m_fullHessianSparsity.nonZeroElementRows;
+                nonZeroElementColumns = m_fullHessianSparsity.nonZeroElementColumns;
 
                 return true;
             }
@@ -1992,7 +2072,7 @@ namespace iDynTree {
                     hessian.resize(static_cast<unsigned int>(numberOfVariables()), static_cast<unsigned int>(numberOfVariables()));
                 }
 
-                m_hessianBlocksSet.reset();
+                m_hessianBlocks.reset();
 
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
                 Eigen::Index constraintIndex = 0;
@@ -2039,7 +2119,7 @@ namespace iDynTree {
 
                             lambdaConstraints = fullLambda.segment(constraintIndex, nc);
 
-                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
+                            if (!(m_ocproblem->constraintsSecondPartialDerivativeWRTControl(mesh->time, m_integrator->dynamicalSystem().lock()->initialState(), m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
                                 std::ostringstream errorMsg;
                                 errorMsg << "Error while evaluating the constraints hessian wrt control at time " << mesh->time << ".";
                                 reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
@@ -2052,7 +2132,7 @@ namespace iDynTree {
 
                             lambdaConstraints = fullLambda.segment(constraintIndex, nc);
 
-                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTState(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_stateHessianBuffer))){
+                            if (!(m_ocproblem->constraintsSecondPartialDerivativeWRTState(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_stateHessianBuffer))){
                                 std::ostringstream errorMsg;
                                 errorMsg << "Error while evaluating the constraints hessian wrt state at time " << mesh->time << ".";
                                 reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
@@ -2061,7 +2141,7 @@ namespace iDynTree {
 
                             setHessianBlock(hessian, m_stateHessianBuffer, mesh->stateIndex, mesh->stateIndex);
 
-                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTStateControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_stateControlHessianBuffer))){
+                            if (!(m_ocproblem->constraintsSecondPartialDerivativeWRTStateControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_stateControlHessianBuffer))){
                                 std::ostringstream errorMsg;
                                 errorMsg << "Error while evaluating the constraints hessian wrt state and control at time " << mesh->time << ".";
                                 reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
@@ -2070,7 +2150,7 @@ namespace iDynTree {
 
                             setHessianBlockAndItsTranspose(hessian, m_stateControlHessianBuffer, mesh->stateIndex, mesh->controlIndex);
 
-                            if (!(m_ocproblem->constraintSecondPartialDerivativeWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
+                            if (!(m_ocproblem->constraintsSecondPartialDerivativeWRTControl(mesh->time, m_collocationStateBuffer[1], m_collocationControlBuffer[1], m_lambdaConstraints, m_controlHessianBuffer))){
                                 std::ostringstream errorMsg;
                                 errorMsg << "Error while evaluating the constraints hessian wrt control at time " << mesh->time << ".";
                                 reportError("MultipleShootingTranscription", "evaluateConstraintsHessian", errorMsg.str().c_str());
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index e6521b0e9fe..b4edfee6078 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -47,10 +47,16 @@ namespace iDynTree {
             MatrixDynSize stateJacobianBuffer, controlJacobianBuffer;
             MatrixDynSize stateHessianBuffer, controlHessianBuffer, mixedHessianBuffer;
             VectorDynSize lambdaBuffer;
-            SparsityStructure stateSparsity;
-            SparsityStructure controlSparsity;
-            bool hasStateSparsity = false;
-            bool hasControlSparsity = false;
+            SparsityStructure stateJacobianSparsity;
+            SparsityStructure controlJacobianSparsity;
+            bool hasStateJacobianSparsity = false;
+            bool hasControlJacobianSparsity = false;
+            SparsityStructure stateHessianSparsity;
+            SparsityStructure controlHessianSparsity;
+            SparsityStructure mixedHessianSparsity;
+            bool hasStateHessianSparsity = false;
+            bool hasControlHessianSparsity = false;
+            bool hasMixedHessianSparsity = false;
 
             template <typename intType>
             void resizeBuffers(intType stateDim, intType controlDim) {
@@ -80,6 +86,12 @@ namespace iDynTree {
             TimeRange timeRange;
             bool isLinear;
             bool isQuadratic;
+            SparsityStructure stateHessianSparsity;
+            SparsityStructure controlHessianSparsity;
+            SparsityStructure mixedHessianSparsity;
+            bool hasStateHessianSparsity = false;
+            bool hasControlHessianSparsity = false;
+            bool hasMixedHessianSparsity = false;
 
             template <typename intType>
             void resizeBuffers(intType stateDim, intType controlDim) {
@@ -109,8 +121,14 @@ namespace iDynTree {
             CostsMap costs;
             bool stateLowerBounded, stateUpperBounded, controlLowerBounded, controlUpperBounded;
             std::shared_ptr<iDynTree::optimalcontrol::TimeVaryingVector> stateLowerBound, stateUpperBound, controlLowerBound, controlUpperBound; //if they are empty is like there is no bound
-            SparsityStructure stateSparsity;
-            SparsityStructure controlSparsity;
+            SparsityStructure stateJacobianSparsity;
+            SparsityStructure controlJacobianSparsity;
+            SparsityStructure constraintsStateHessianSparsity;
+            SparsityStructure constraintsControlHessianSparsity;
+            SparsityStructure constraintsMixedHessianSparsity;
+            SparsityStructure costsStateHessianSparsity;
+            SparsityStructure costsControlHessianSparsity;
+            SparsityStructure costsMixedHessianSparsity;
             std::vector<std::string> mayerCostnames;
             std::vector<TimeRange> constraintsTimeRanges, costTimeRanges;
             std::vector<size_t> linearConstraintIndeces;
@@ -268,8 +286,8 @@ namespace iDynTree {
             }
             newGroup->lambdaBuffer.resize(groupOfConstraints->constraintsDimension());
 
-            newGroup->hasStateSparsity = groupOfConstraints->constraintJacobianWRTStateSparsity(newGroup->stateSparsity); //needed to allocate memory in advance
-            newGroup->hasControlSparsity = groupOfConstraints->constraintJacobianWRTControlSparsity(newGroup->controlSparsity); //needed to allocate memory in advance
+            newGroup->hasStateJacobianSparsity = groupOfConstraints->constraintJacobianWRTStateSparsity(newGroup->stateJacobianSparsity); //needed to allocate memory in advance
+            newGroup->hasControlJacobianSparsity = groupOfConstraints->constraintJacobianWRTControlSparsity(newGroup->controlJacobianSparsity); //needed to allocate memory in advance
 
             std::pair< ConstraintsGroupsMap::iterator, bool> groupResult;
             groupResult = m_pimpl->constraintsGroups.insert(std::pair< std::string, BufferedGroup_ptr>(groupOfConstraints->name(), newGroup));
@@ -281,9 +299,9 @@ namespace iDynTree {
                 return false;
             }
 
-            m_pimpl->stateSparsity.resize(m_pimpl->stateSparsity.size() + newGroup->stateSparsity.size()); //needed to allocate memory in advance
+            m_pimpl->stateJacobianSparsity.resize(m_pimpl->stateJacobianSparsity.size() + newGroup->stateJacobianSparsity.size()); //needed to allocate memory in advance
 
-            m_pimpl->controlSparsity.resize(m_pimpl->controlSparsity.size() + newGroup->controlSparsity.size()); //needed to allocate memory in advance
+            m_pimpl->controlJacobianSparsity.resize(m_pimpl->controlJacobianSparsity.size() + newGroup->controlJacobianSparsity.size()); //needed to allocate memory in advance
 
             return true;
         }
@@ -1101,6 +1119,114 @@ namespace iDynTree {
             return true;
         }
 
+        bool OptimalControlProblem::costsSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            bool allHaveSparsity = true;
+
+            auto cost = m_pimpl->costs.begin();
+
+            while (allHaveSparsity && cost != m_pimpl->costs.end()) {
+                cost->second.hasStateHessianSparsity =
+                    cost->second.cost->costSecondPartialDerivativeWRTStateSparsity(cost->second.stateHessianSparsity);
+                allHaveSparsity &= cost->second.hasStateHessianSparsity;
+                cost++;
+            }
+
+            if (allHaveSparsity) {
+                m_pimpl->costsStateHessianSparsity.clear();
+
+                for (auto& costPtr : m_pimpl->costs) {
+                    m_pimpl->costsStateHessianSparsity.merge(costPtr.second.stateHessianSparsity);
+                }
+
+            } else {
+                if (m_pimpl->dynamicalSystem) {
+                    m_pimpl->costsStateHessianSparsity.clear();
+                    m_pimpl->costsStateHessianSparsity.addDenseBlock(0, 0, m_pimpl->dynamicalSystem->stateSpaceSize(), m_pimpl->dynamicalSystem->stateSpaceSize());
+                } else {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Not all costs have the hessian sparsity defined and no dynamical system has been provided yet. Cannot determine the sparsity since the state dimension is unknown.";
+                    reportError("OptimalControlProblem", "costsSecondPartialDerivativeWRTStateSparsity", errorMsg.str().c_str());
+                    return false;
+                }
+            }
+
+            stateSparsity = m_pimpl->costsStateHessianSparsity;
+            return true;
+        }
+
+        bool OptimalControlProblem::costsSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            bool allHaveSparsity = true;
+
+            auto cost = m_pimpl->costs.begin();
+
+            while (allHaveSparsity && cost != m_pimpl->costs.end()) {
+                cost->second.hasControlHessianSparsity =
+                    cost->second.cost->costSecondPartialDerivativeWRTControlSparsity(cost->second.controlHessianSparsity);
+                allHaveSparsity &= cost->second.hasControlHessianSparsity;
+                cost++;
+            }
+
+            if (allHaveSparsity) {
+                m_pimpl->costsControlHessianSparsity.clear();
+
+                for (auto& costPtr : m_pimpl->costs) {
+                    m_pimpl->costsControlHessianSparsity.merge(costPtr.second.controlHessianSparsity);
+                }
+
+            } else {
+                if (m_pimpl->dynamicalSystem) {
+                    m_pimpl->costsControlHessianSparsity.clear();
+                    m_pimpl->costsControlHessianSparsity.addDenseBlock(0, 0, m_pimpl->dynamicalSystem->controlSpaceSize(), m_pimpl->dynamicalSystem->controlSpaceSize());
+                } else {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Not all costs have the hessian sparsity defined and no dynamical system has been provided yet. Cannot determine the sparsity since the control dimension is unknown.";
+                    reportError("OptimalControlProblem", "costsSecondPartialDerivativeWRTControlSparsity", errorMsg.str().c_str());
+                    return false;
+                }
+            }
+
+            controlSparsity = m_pimpl->costsControlHessianSparsity;
+            return true;
+        }
+
+        bool OptimalControlProblem::costsSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            bool allHaveSparsity = true;
+
+            auto cost = m_pimpl->costs.begin();
+
+            while (allHaveSparsity && cost != m_pimpl->costs.end()) {
+                cost->second.hasMixedHessianSparsity =
+                    cost->second.cost->costSecondPartialDerivativeWRTStateControlSparsity(cost->second.mixedHessianSparsity);
+                allHaveSparsity &= cost->second.hasMixedHessianSparsity;
+                cost++;
+            }
+
+            if (allHaveSparsity) {
+                m_pimpl->costsMixedHessianSparsity.clear();
+
+                for (auto& costPtr : m_pimpl->costs) {
+                    m_pimpl->costsMixedHessianSparsity.merge(costPtr.second.mixedHessianSparsity);
+                }
+
+            } else {
+                if (m_pimpl->dynamicalSystem) {
+                    m_pimpl->costsMixedHessianSparsity.clear();
+                    m_pimpl->costsMixedHessianSparsity.addDenseBlock(0, 0, m_pimpl->dynamicalSystem->stateSpaceSize(), m_pimpl->dynamicalSystem->controlSpaceSize());
+                } else {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Not all costs have the hessian sparsity defined and no dynamical system has been provided yet. Cannot determine the sparsity since the state and control dimensions are unknown.";
+                    reportError("OptimalControlProblem", "costsSecondPartialDerivativeWRTStateControlSparsity", errorMsg.str().c_str());
+                    return false;
+                }
+            }
+
+            stateControlSparsity = m_pimpl->costsMixedHessianSparsity;
+            return true;
+        }
+
         bool OptimalControlProblem::constraintsEvaluation(double time, const VectorDynSize &state, const VectorDynSize &control, VectorDynSize &constraintsValue)
         {
             if (constraintsValue.size() != getConstraintsDimension()) {
@@ -1238,23 +1364,23 @@ namespace iDynTree {
             return true;
         }
 
-        bool OptimalControlProblem::constraintJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
+        bool OptimalControlProblem::constraintsJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
         {
             size_t nonZeroIndexState = 0;
             size_t offset = 0;
             for (auto& group : m_pimpl->constraintsGroups){
 
-                group.second->hasStateSparsity = group.second->group_ptr->constraintJacobianWRTStateSparsity(group.second->stateSparsity);
+                group.second->hasStateJacobianSparsity = group.second->group_ptr->constraintJacobianWRTStateSparsity(group.second->stateJacobianSparsity);
 
-                if (group.second->hasStateSparsity) {
+                if (group.second->hasStateJacobianSparsity) {
 
-                    if (m_pimpl->stateSparsity.size() < (nonZeroIndexState + group.second->stateSparsity.size())) {
-                        m_pimpl->stateSparsity.resize(nonZeroIndexState + group.second->stateSparsity.size());
+                    if (m_pimpl->stateJacobianSparsity.size() < (nonZeroIndexState + group.second->stateJacobianSparsity.size())) {
+                        m_pimpl->stateJacobianSparsity.resize(nonZeroIndexState + group.second->stateJacobianSparsity.size());
                     }
 
-                    for (size_t i = 0; i < group.second->stateSparsity.size(); ++i) {
-                        m_pimpl->stateSparsity.nonZeroElementRows[nonZeroIndexState]    = group.second->stateSparsity.nonZeroElementRows[i] + offset;
-                        m_pimpl->stateSparsity.nonZeroElementColumns[nonZeroIndexState] = group.second->stateSparsity.nonZeroElementColumns[i];
+                    for (size_t i = 0; i < group.second->stateJacobianSparsity.size(); ++i) {
+                        m_pimpl->stateJacobianSparsity.nonZeroElementRows[nonZeroIndexState]    = group.second->stateJacobianSparsity.nonZeroElementRows[i] + offset;
+                        m_pimpl->stateJacobianSparsity.nonZeroElementColumns[nonZeroIndexState] = group.second->stateJacobianSparsity.nonZeroElementColumns[i];
                         nonZeroIndexState++;
                     }
 
@@ -1266,14 +1392,14 @@ namespace iDynTree {
                         size_t cols = group.second->stateJacobianBuffer.cols();
                         size_t nonZeros = rows * cols;
 
-                        if (m_pimpl->stateSparsity.size() < (nonZeroIndexState + nonZeros)) {
-                            m_pimpl->stateSparsity.resize(nonZeroIndexState + nonZeros);
+                        if (m_pimpl->stateJacobianSparsity.size() < (nonZeroIndexState + nonZeros)) {
+                            m_pimpl->stateJacobianSparsity.resize(nonZeroIndexState + nonZeros);
                         }
 
                         for (size_t i = 0; i < rows; ++i) {
                             for (size_t j = 0; j < cols; ++j) {
-                                m_pimpl->stateSparsity.nonZeroElementRows[nonZeroIndexState]    = i + offset;
-                                m_pimpl->stateSparsity.nonZeroElementColumns[nonZeroIndexState] = j;
+                                m_pimpl->stateJacobianSparsity.nonZeroElementRows[nonZeroIndexState]    = i + offset;
+                                m_pimpl->stateJacobianSparsity.nonZeroElementColumns[nonZeroIndexState] = j;
                                 nonZeroIndexState++;
                             }
                         }
@@ -1287,31 +1413,31 @@ namespace iDynTree {
 
                 offset += group.second->stateJacobianBuffer.rows();
             }
-            m_pimpl->stateSparsity.resize(nonZeroIndexState); //remove leftovers
+            m_pimpl->stateJacobianSparsity.resize(nonZeroIndexState); //remove leftovers
 
 
-            stateSparsity = m_pimpl->stateSparsity;
+            stateSparsity = m_pimpl->stateJacobianSparsity;
 
             return true;
         }
 
-        bool OptimalControlProblem::constraintJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
+        bool OptimalControlProblem::constraintsJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
         {
             size_t nonZeroIndexControl = 0;
             size_t offset = 0;
             for (auto& group : m_pimpl->constraintsGroups){
 
-                group.second->hasControlSparsity = group.second->group_ptr->constraintJacobianWRTControlSparsity(group.second->controlSparsity);
+                group.second->hasControlJacobianSparsity = group.second->group_ptr->constraintJacobianWRTControlSparsity(group.second->controlJacobianSparsity);
 
-                if (group.second->hasControlSparsity) {
+                if (group.second->hasControlJacobianSparsity) {
 
-                    if (m_pimpl->controlSparsity.size() < (nonZeroIndexControl + group.second->controlSparsity.size())) {
-                        m_pimpl->controlSparsity.resize(nonZeroIndexControl + group.second->controlSparsity.size());
+                    if (m_pimpl->controlJacobianSparsity.size() < (nonZeroIndexControl + group.second->controlJacobianSparsity.size())) {
+                        m_pimpl->controlJacobianSparsity.resize(nonZeroIndexControl + group.second->controlJacobianSparsity.size());
                     }
 
-                    for (size_t i = 0; i < group.second->controlSparsity.size(); ++i) {
-                        m_pimpl->controlSparsity.nonZeroElementRows[nonZeroIndexControl]    = group.second->controlSparsity.nonZeroElementRows[i] + offset;
-                        m_pimpl->controlSparsity.nonZeroElementColumns[nonZeroIndexControl] = group.second->controlSparsity.nonZeroElementColumns[i];
+                    for (size_t i = 0; i < group.second->controlJacobianSparsity.size(); ++i) {
+                        m_pimpl->controlJacobianSparsity.nonZeroElementRows[nonZeroIndexControl]    = group.second->controlJacobianSparsity.nonZeroElementRows[i] + offset;
+                        m_pimpl->controlJacobianSparsity.nonZeroElementColumns[nonZeroIndexControl] = group.second->controlJacobianSparsity.nonZeroElementColumns[i];
                         nonZeroIndexControl++;
                     }
 
@@ -1323,14 +1449,14 @@ namespace iDynTree {
                         size_t cols = group.second->controlJacobianBuffer.cols();
                         size_t nonZeros = rows * cols;
 
-                        if (m_pimpl->controlSparsity.size() < (nonZeroIndexControl + nonZeros)) {
-                            m_pimpl->controlSparsity.resize(nonZeroIndexControl + nonZeros);
+                        if (m_pimpl->controlJacobianSparsity.size() < (nonZeroIndexControl + nonZeros)) {
+                            m_pimpl->controlJacobianSparsity.resize(nonZeroIndexControl + nonZeros);
                         }
 
                         for (size_t i = 0; i < rows; ++i) {
                             for (size_t j = 0; j < cols; ++j) {
-                                m_pimpl->controlSparsity.nonZeroElementRows[nonZeroIndexControl]    = i + offset;
-                                m_pimpl->controlSparsity.nonZeroElementColumns[nonZeroIndexControl] = j;
+                                m_pimpl->controlJacobianSparsity.nonZeroElementRows[nonZeroIndexControl]    = i + offset;
+                                m_pimpl->controlJacobianSparsity.nonZeroElementColumns[nonZeroIndexControl] = j;
                                 nonZeroIndexControl++;
                             }
                         }
@@ -1344,15 +1470,15 @@ namespace iDynTree {
 
                 offset += group.second->controlJacobianBuffer.rows();
             }
-            m_pimpl->controlSparsity.resize(nonZeroIndexControl); //remove leftovers
+            m_pimpl->controlJacobianSparsity.resize(nonZeroIndexControl); //remove leftovers
 
 
-            controlSparsity = m_pimpl->controlSparsity;
+            controlSparsity = m_pimpl->controlJacobianSparsity;
 
             return true;
         }
 
-        bool OptimalControlProblem::constraintSecondPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        bool OptimalControlProblem::constraintsSecondPartialDerivativeWRTState(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
         {
             if ((hessian.rows() != state.size()) || (hessian.cols() != state.size())) {
                 hessian.resize(state.size(), state.size());
@@ -1387,7 +1513,7 @@ namespace iDynTree {
             return true;
         }
 
-        bool OptimalControlProblem::constraintSecondPartialDerivativeWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        bool OptimalControlProblem::constraintsSecondPartialDerivativeWRTControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
         {
             if ((hessian.rows() != control.size()) || (hessian.cols() != control.size())) {
                 hessian.resize(control.size(), control.size());
@@ -1420,7 +1546,7 @@ namespace iDynTree {
             return true;
         }
 
-        bool OptimalControlProblem::constraintSecondPartialDerivativeWRTStateControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
+        bool OptimalControlProblem::constraintsSecondPartialDerivativeWRTStateControl(double time, const VectorDynSize &state, const VectorDynSize &control, const VectorDynSize &lambda, MatrixDynSize &hessian)
         {
             if ((hessian.rows() != state.size()) || (hessian.cols() != control.size())) {
                 hessian.resize(state.size(), control.size());
@@ -1454,5 +1580,113 @@ namespace iDynTree {
             return true;
         }
 
+        bool OptimalControlProblem::constraintsSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            bool allHaveSparsity = true;
+
+            auto group = m_pimpl->constraintsGroups.begin();
+
+            while (allHaveSparsity && group != m_pimpl->constraintsGroups.end()) {
+                group->second->hasStateHessianSparsity =
+                    group->second->group_ptr->constraintsSecondPartialDerivativeWRTStateSparsity(group->second->stateHessianSparsity);
+                allHaveSparsity &= group->second->hasStateHessianSparsity;
+                group++;
+            }
+
+            if (allHaveSparsity) {
+                m_pimpl->constraintsStateHessianSparsity.clear();
+
+                for (auto& groupPtr : m_pimpl->constraintsGroups) {
+                    m_pimpl->constraintsStateHessianSparsity.merge(groupPtr.second->stateHessianSparsity);
+                }
+
+            } else {
+                if (m_pimpl->dynamicalSystem) {
+                    m_pimpl->constraintsStateHessianSparsity.clear();
+                    m_pimpl->constraintsStateHessianSparsity.addDenseBlock(0, 0, m_pimpl->dynamicalSystem->stateSpaceSize(), m_pimpl->dynamicalSystem->stateSpaceSize());
+                } else {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Not all groups have the hessian sparsity defined and no dynamical system has been provided yet. Cannot determine the sparsity since the state dimension is unknown.";
+                    reportError("OptimalControlProblem", "constraintsSecondPartialDerivativeWRTStateSparsity", errorMsg.str().c_str());
+                    return false;
+                }
+            }
+
+            stateSparsity = m_pimpl->constraintsStateHessianSparsity;
+            return true;
+        }
+
+        bool OptimalControlProblem::constraintsSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            bool allHaveSparsity = true;
+
+            auto group = m_pimpl->constraintsGroups.begin();
+
+            while (allHaveSparsity && group != m_pimpl->constraintsGroups.end()) {
+                group->second->hasControlHessianSparsity =
+                    group->second->group_ptr->constraintsSecondPartialDerivativeWRTControlSparsity(group->second->controlHessianSparsity);
+                allHaveSparsity &= group->second->hasControlHessianSparsity;
+                group++;
+            }
+
+            if (allHaveSparsity) {
+                m_pimpl->constraintsControlHessianSparsity.clear();
+
+                for (auto& groupPtr : m_pimpl->constraintsGroups) {
+                    m_pimpl->constraintsControlHessianSparsity.merge(groupPtr.second->controlHessianSparsity);
+                }
+
+            } else {
+                if (m_pimpl->dynamicalSystem) {
+                    m_pimpl->constraintsControlHessianSparsity.clear();
+                    m_pimpl->constraintsControlHessianSparsity.addDenseBlock(0, 0, m_pimpl->dynamicalSystem->controlSpaceSize(), m_pimpl->dynamicalSystem->controlSpaceSize());
+                } else {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Not all groups have the hessian sparsity defined and no dynamical system has been provided yet. Cannot determine the sparsity since the control dimension is unknown.";
+                    reportError("OptimalControlProblem", "constraintsSecondPartialDerivativeWRTControlSparsity", errorMsg.str().c_str());
+                    return false;
+                }
+            }
+
+            controlSparsity = m_pimpl->constraintsControlHessianSparsity;
+            return true;
+        }
+
+        bool OptimalControlProblem::constraintsSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            bool allHaveSparsity = true;
+
+            auto group = m_pimpl->constraintsGroups.begin();
+
+            while (allHaveSparsity && group != m_pimpl->constraintsGroups.end()) {
+                group->second->hasMixedHessianSparsity =
+                    group->second->group_ptr->constraintsSecondPartialDerivativeWRTStateControlSparsity(group->second->mixedHessianSparsity);
+                allHaveSparsity &= group->second->hasMixedHessianSparsity;
+                group++;
+            }
+
+            if (allHaveSparsity) {
+                m_pimpl->constraintsMixedHessianSparsity.clear();
+
+                for (auto& groupPtr : m_pimpl->constraintsGroups) {
+                    m_pimpl->constraintsMixedHessianSparsity.merge(groupPtr.second->mixedHessianSparsity);
+                }
+
+            } else {
+                if (m_pimpl->dynamicalSystem) {
+                    m_pimpl->constraintsMixedHessianSparsity.clear();
+                    m_pimpl->constraintsMixedHessianSparsity.addDenseBlock(0, 0, m_pimpl->dynamicalSystem->stateSpaceSize(), m_pimpl->dynamicalSystem->controlSpaceSize());
+                } else {
+                    std::ostringstream errorMsg;
+                    errorMsg << "Not all groups have the hessian sparsity defined and no dynamical system has been provided yet. Cannot determine the sparsity since both the state and the control dimensions are unknown.";
+                    reportError("OptimalControlProblem", "constraintsSecondPartialDerivativeWRTStateControlSparsity", errorMsg.str().c_str());
+                    return false;
+                }
+            }
+
+            stateControlSparsity = m_pimpl->constraintsMixedHessianSparsity;
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/QuadraticCost.cpp b/src/optimalcontrol/src/QuadraticCost.cpp
index 6101986889b..e493d2b1000 100644
--- a/src/optimalcontrol/src/QuadraticCost.cpp
+++ b/src/optimalcontrol/src/QuadraticCost.cpp
@@ -25,7 +25,9 @@ namespace iDynTree {
 
         QuadraticCost::QuadraticCost(const std::string &costName)
             : QuadraticLikeCost(costName)
-        { }
+        {
+            m_hasSecondPartialDerivativeWRTStateControlSparsity = true;
+        }
 
         QuadraticCost::~QuadraticCost()
         { }
@@ -127,5 +129,27 @@ namespace iDynTree {
             return true;
         }
 
+        bool iDynTree::optimalcontrol::QuadraticCost::setStateHessianSparsity(const SparsityStructure &stateSparsity)
+        {
+            if (!stateSparsity.isValid()) {
+                reportError("QuadraticCost", "setStateHessianSparsity", "The sparsity vectors have different number of entries.");
+                return false;
+            }
+            m_hasSecondPartialDerivativeWRTStateSparsity = true;
+            m_secondPartialDerivativeWRTStateSparsity = stateSparsity;
+            return true;
+        }
+
+        bool QuadraticCost::setControlHessianSparsity(const SparsityStructure &controlSparsity)
+        {
+            if (!controlSparsity.isValid()) {
+                reportError("QuadraticCost", "setControlHessianSparsity", "The sparsity vectors have different number of entries.");
+                return false;
+            }
+            m_hasSecondPartialDerivativeWRTControlSparsity = true;
+            m_secondPartialDerivativeWRTControlSparsity = controlSparsity;
+            return true;
+        }
+
     }
 }
diff --git a/src/optimalcontrol/src/QuadraticLikeCost.cpp b/src/optimalcontrol/src/QuadraticLikeCost.cpp
index 95df3e3f476..89f37dfa7a6 100644
--- a/src/optimalcontrol/src/QuadraticLikeCost.cpp
+++ b/src/optimalcontrol/src/QuadraticLikeCost.cpp
@@ -32,6 +32,9 @@ namespace iDynTree {
             , m_timeVaryingControlHessian(nullptr)
             , m_timeVaryingControlGradient(nullptr)
             , m_timeVaryingControlCostBias(nullptr)
+            , m_hasSecondPartialDerivativeWRTStateSparsity(false)
+            , m_hasSecondPartialDerivativeWRTControlSparsity(false)
+            , m_hasSecondPartialDerivativeWRTStateControlSparsity(false)
         { }
 
         QuadraticLikeCost::~QuadraticLikeCost()
@@ -394,6 +397,33 @@ namespace iDynTree {
             return true;
         }
 
+        bool QuadraticLikeCost::costSecondPartialDerivativeWRTStateSparsity(SparsityStructure &stateSparsity)
+        {
+            if (!m_hasSecondPartialDerivativeWRTStateSparsity) {
+                return false;
+            }
+            stateSparsity = m_secondPartialDerivativeWRTStateSparsity;
+            return true;
+        }
+
+        bool QuadraticLikeCost::costSecondPartialDerivativeWRTStateControlSparsity(SparsityStructure &stateControlSparsity)
+        {
+            if (!m_hasSecondPartialDerivativeWRTStateControlSparsity) {
+                return false;
+            }
+            stateControlSparsity = m_secondPartialDerivativeWRTStateControlSparsity;
+            return true;
+        }
+
+        bool QuadraticLikeCost::costSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
+        {
+            if (!m_hasSecondPartialDerivativeWRTControlSparsity) {
+                return false;
+            }
+            controlSparsity = m_secondPartialDerivativeWRTControlSparsity;
+            return true;
+        }
+
 
     }
 }
diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp
index 8e7f1696e4b..da7ac69dc62 100644
--- a/src/optimalcontrol/src/SparsityStructure.cpp
+++ b/src/optimalcontrol/src/SparsityStructure.cpp
@@ -74,6 +74,20 @@ void iDynTree::optimalcontrol::SparsityStructure::addIdentityBlock(size_t startR
     }
 }
 
+bool iDynTree::optimalcontrol::SparsityStructure::addBlock(size_t startRow, size_t startColumn, const iDynTree::optimalcontrol::SparsityStructure &other)
+{
+    if (!other.isValid()) {
+        reportError("SparsityStructure", "addBlock", "The other SparsityStructure vectors have different size.");
+        return false;
+    }
+
+    for (size_t i = 0; i < other.size(); ++i) {
+        addNonZeroIfNotPresent(startRow + other.nonZeroElementRows[i], startColumn + other.nonZeroElementColumns[i]);
+    }
+
+    return true;
+}
+
 void iDynTree::optimalcontrol::SparsityStructure::addNonZeroIfNotPresent(size_t newRow, size_t newCol)
 {
     if (!isValuePresent(newRow, newCol)) {
diff --git a/src/optimalcontrol/tests/OCProblemTest.cpp b/src/optimalcontrol/tests/OCProblemTest.cpp
index 8717e64e47a..c4fd15141e9 100644
--- a/src/optimalcontrol/tests/OCProblemTest.cpp
+++ b/src/optimalcontrol/tests/OCProblemTest.cpp
@@ -390,8 +390,8 @@ int main() {
     ASSERT_IS_FALSE(problem.isFeasiblePoint(4.0, testState, testControl));
 
     iDynTree::optimalcontrol::SparsityStructure stateSparsity, controlSparsity;
-    ASSERT_IS_TRUE(problem.constraintJacobianWRTStateSparsity(stateSparsity));
-    ASSERT_IS_TRUE(problem.constraintJacobianWRTControlSparsity(controlSparsity));
+    ASSERT_IS_TRUE(problem.constraintsJacobianWRTStateSparsity(stateSparsity));
+    ASSERT_IS_TRUE(problem.constraintsJacobianWRTControlSparsity(controlSparsity));
     iDynTree::MatrixDynSize stateSparsityCheck, controlSparsityCheck, stateZeroCheck, controlZeroCheck;
     stateSparsityCheck = obtainedStatejac;
     stateZeroCheck = stateSparsityCheck;

From 983c241e4240a99ca563db07bbb78010912a3a15 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 8 May 2019 23:21:48 +0200
Subject: [PATCH 112/194] The SparsityStructure class uses an unordered_set to
 determine if a zero is present or not.

This allow to check for duplications in constant time instead of linear. As a consequence, the sparsity vectors cannot be accessed directly.
---
 .../include/iDynTree/SparsityStructure.h      | 27 +++++--
 src/optimalcontrol/src/ForwardEuler.cpp       | 11 ++-
 .../src/ImplicitTrapezoidal.cpp               |  2 +-
 .../src/MultipleShootingSolver.cpp            | 34 +++------
 .../src/OptimalControlProblem.cpp             | 48 +++----------
 src/optimalcontrol/src/SparsityStructure.cpp  | 70 +++++++++++++------
 .../tests/MultipleShootingTest.cpp            | 10 +--
 src/optimalcontrol/tests/OCProblemTest.cpp    | 18 ++---
 8 files changed, 109 insertions(+), 111 deletions(-)

diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
index a0e122f8cae..15097d55350 100644
--- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -20,17 +20,28 @@
 #include <iDynTree/Core/Utils.h>
 #include <vector>
 #include <cstddef>
+#include <unordered_set>
 
 namespace iDynTree {
     namespace optimalcontrol {
         class SparsityStructure;
+
+        struct NonZero {
+            size_t row;
+            size_t col;
+        };
     }
 }
 
 class iDynTree::optimalcontrol::SparsityStructure {
+    std::unordered_set<std::string> m_register;
+
+    void addNonZero(size_t row, size_t col);
+
+    std::vector<size_t> m_nonZeroElementRows;
+    std::vector<size_t> m_nonZeroElementColumns;
+
 public:
-    std::vector<size_t> nonZeroElementRows;
-    std::vector<size_t> nonZeroElementColumns;
 
     SparsityStructure();
 
@@ -46,17 +57,25 @@ class iDynTree::optimalcontrol::SparsityStructure {
 
     bool addBlock(size_t startRow, size_t startColumn, const SparsityStructure& other);
 
-    void addNonZeroIfNotPresent(size_t newRow, size_t newCol);
+    void add(size_t newRow, size_t newCol);
+
+    void add(NonZero newElement);
 
     bool isValuePresent(size_t row, size_t col) const;
 
-    void resize(size_t newSize);
+    void reserve(size_t newSize);
 
     void clear();
 
     size_t size() const;
 
     bool isValid() const;
+
+    NonZero operator[](size_t index) const;
+
+    const std::vector<size_t>& nonZeroElementRows() const;
+
+    const std::vector<size_t>& nonZeroElementColumns() const;
 };
 
 #endif // IDYNTREE_OPTIMALCONTROL_SPARSITYSTRUCTURE_H
diff --git a/src/optimalcontrol/src/ForwardEuler.cpp b/src/optimalcontrol/src/ForwardEuler.cpp
index c7d8981d81b..3837542f658 100644
--- a/src/optimalcontrol/src/ForwardEuler.cpp
+++ b/src/optimalcontrol/src/ForwardEuler.cpp
@@ -64,21 +64,18 @@ namespace iDynTree {
                 m_controlJacobianSparsity.resize(2);
 
                 if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0])) {
-                    m_stateJacobianSparsity[1].nonZeroElementRows.clear();
-                    m_stateJacobianSparsity[1].nonZeroElementColumns.clear();
+                    m_stateJacobianSparsity[1].clear();
 
                     for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
-                        m_stateJacobianSparsity[0].addNonZeroIfNotPresent(i, i);
-                        m_stateJacobianSparsity[1].nonZeroElementRows.push_back(i);
-                        m_stateJacobianSparsity[1].nonZeroElementColumns.push_back(i);
+                        m_stateJacobianSparsity[0].add(i, i);
+                        m_stateJacobianSparsity[1].add(i, i);
                     }
 
                     m_hasStateJacobianSparsity = true;
                 }
 
                 if (m_dynamicalSystem_ptr->dynamicsControlFirstDerivativeSparsity(m_controlJacobianSparsity[0])) {
-                    m_controlJacobianSparsity[1].nonZeroElementRows.clear();
-                    m_controlJacobianSparsity[1].nonZeroElementColumns.clear();
+                    m_controlJacobianSparsity[1].clear();
 
                     m_hasControlJacobianSparsity = true;
                 }
diff --git a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
index 0a2b875e3bc..ffe6c56acc8 100644
--- a/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
+++ b/src/optimalcontrol/src/ImplicitTrapezoidal.cpp
@@ -69,7 +69,7 @@ namespace iDynTree {
                 if (m_dynamicalSystem_ptr->dynamicsStateFirstDerivativeSparsity(m_stateJacobianSparsity[0])) {
 
                     for (size_t i = 0; i < m_dynamicalSystem_ptr->stateSpaceSize(); ++i) {
-                        m_stateJacobianSparsity[0].addNonZeroIfNotPresent(i, i);
+                        m_stateJacobianSparsity[0].add(i, i);
                     }
                     m_stateJacobianSparsity[1] = m_stateJacobianSparsity[0];
 
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 11bee173d28..94d2782aba4 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -252,8 +252,8 @@ namespace iDynTree {
 
             void addJacobianBlock(size_t initRow, size_t initCol, const SparsityStructure& sparsity){
                 for (size_t i = 0; i < sparsity.size(); ++i) {
-                    addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + sparsity.nonZeroElementRows[i]);
-                    addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + sparsity.nonZeroElementColumns[i]);
+                    addNonZero(m_jacobianNZRows, m_jacobianNonZeros, initRow + sparsity[i].row);
+                    addNonZero(m_jacobianNZCols, m_jacobianNonZeros, initCol + sparsity[i].col);
                     m_jacobianNonZeros++;
                 }
             }
@@ -285,7 +285,7 @@ namespace iDynTree {
                 SparsityStructure& transposeSparsity = m_hessianBlocks.blockSparsity(initCol, initRow);
 
                 for (size_t i = 0; i < sparsity.size(); ++i){
-                    transposeSparsity.addNonZeroIfNotPresent(sparsity.nonZeroElementColumns[i], sparsity.nonZeroElementRows[i]);
+                    transposeSparsity.add(sparsity[i]);
                 }
             }
 
@@ -318,27 +318,9 @@ namespace iDynTree {
             }
 
             void mergeSparsityVectors(const std::vector<SparsityStructure>& original, SparsityStructure& merged) {
-                const std::vector<size_t>& firstRows = original[0].nonZeroElementRows;
-                const std::vector<size_t>& firstCols = original[0].nonZeroElementColumns;
-                const std::vector<size_t>& secondRows = original[1].nonZeroElementRows;
-                const std::vector<size_t>& secondCols = original[1].nonZeroElementColumns;
-                size_t mergedNonZeros = 0;
-                for (size_t i = 0; i < firstRows.size(); ++i) {
-                    addNonZero(merged.nonZeroElementRows, mergedNonZeros, firstRows[i]);
-                    addNonZero(merged.nonZeroElementColumns, mergedNonZeros, firstCols[i]);
-                    mergedNonZeros++;
-                }
-
-                for (size_t j = 0; j < secondRows.size(); ++j) {
-                    bool duplicate = original[0].isValuePresent(secondRows[j], secondCols[j]);
-
-                    if (!duplicate) {
-                        addNonZero(merged.nonZeroElementRows, mergedNonZeros, secondRows[j]);
-                        addNonZero(merged.nonZeroElementColumns, mergedNonZeros, secondCols[j]);
-                        mergedNonZeros++;
-                    }
-                }
-                merged.resize(mergedNonZeros);
+                merged.clear();
+                merged = original[0];
+                merged.merge(original[1]);
             }
 
             void allocateBuffers(){
@@ -1629,8 +1611,8 @@ namespace iDynTree {
                     return false;
                 }
 
-                nonZeroElementRows = m_fullHessianSparsity.nonZeroElementRows;
-                nonZeroElementColumns = m_fullHessianSparsity.nonZeroElementColumns;
+                nonZeroElementRows = m_fullHessianSparsity.nonZeroElementRows();
+                nonZeroElementColumns = m_fullHessianSparsity.nonZeroElementColumns();
 
                 return true;
             }
diff --git a/src/optimalcontrol/src/OptimalControlProblem.cpp b/src/optimalcontrol/src/OptimalControlProblem.cpp
index b4edfee6078..9cf29c9950c 100644
--- a/src/optimalcontrol/src/OptimalControlProblem.cpp
+++ b/src/optimalcontrol/src/OptimalControlProblem.cpp
@@ -299,9 +299,9 @@ namespace iDynTree {
                 return false;
             }
 
-            m_pimpl->stateJacobianSparsity.resize(m_pimpl->stateJacobianSparsity.size() + newGroup->stateJacobianSparsity.size()); //needed to allocate memory in advance
+            m_pimpl->stateJacobianSparsity.reserve(m_pimpl->stateJacobianSparsity.size() + newGroup->stateJacobianSparsity.size()); //needed to allocate memory in advance
 
-            m_pimpl->controlJacobianSparsity.resize(m_pimpl->controlJacobianSparsity.size() + newGroup->controlJacobianSparsity.size()); //needed to allocate memory in advance
+            m_pimpl->controlJacobianSparsity.reserve(m_pimpl->controlJacobianSparsity.size() + newGroup->controlJacobianSparsity.size()); //needed to allocate memory in advance
 
             return true;
         }
@@ -1366,22 +1366,17 @@ namespace iDynTree {
 
         bool OptimalControlProblem::constraintsJacobianWRTStateSparsity(SparsityStructure &stateSparsity)
         {
-            size_t nonZeroIndexState = 0;
             size_t offset = 0;
+            m_pimpl->stateJacobianSparsity.clear();
             for (auto& group : m_pimpl->constraintsGroups){
 
                 group.second->hasStateJacobianSparsity = group.second->group_ptr->constraintJacobianWRTStateSparsity(group.second->stateJacobianSparsity);
 
                 if (group.second->hasStateJacobianSparsity) {
 
-                    if (m_pimpl->stateJacobianSparsity.size() < (nonZeroIndexState + group.second->stateJacobianSparsity.size())) {
-                        m_pimpl->stateJacobianSparsity.resize(nonZeroIndexState + group.second->stateJacobianSparsity.size());
-                    }
-
                     for (size_t i = 0; i < group.second->stateJacobianSparsity.size(); ++i) {
-                        m_pimpl->stateJacobianSparsity.nonZeroElementRows[nonZeroIndexState]    = group.second->stateJacobianSparsity.nonZeroElementRows[i] + offset;
-                        m_pimpl->stateJacobianSparsity.nonZeroElementColumns[nonZeroIndexState] = group.second->stateJacobianSparsity.nonZeroElementColumns[i];
-                        nonZeroIndexState++;
+                        m_pimpl->stateJacobianSparsity.add(group.second->stateJacobianSparsity[i].row + offset,
+                                                           group.second->stateJacobianSparsity[i].col);
                     }
 
                 } else {
@@ -1390,17 +1385,10 @@ namespace iDynTree {
 
                         size_t rows = group.second->stateJacobianBuffer.rows();
                         size_t cols = group.second->stateJacobianBuffer.cols();
-                        size_t nonZeros = rows * cols;
-
-                        if (m_pimpl->stateJacobianSparsity.size() < (nonZeroIndexState + nonZeros)) {
-                            m_pimpl->stateJacobianSparsity.resize(nonZeroIndexState + nonZeros);
-                        }
 
                         for (size_t i = 0; i < rows; ++i) {
                             for (size_t j = 0; j < cols; ++j) {
-                                m_pimpl->stateJacobianSparsity.nonZeroElementRows[nonZeroIndexState]    = i + offset;
-                                m_pimpl->stateJacobianSparsity.nonZeroElementColumns[nonZeroIndexState] = j;
-                                nonZeroIndexState++;
+                                m_pimpl->stateJacobianSparsity.add(i + offset, j);
                             }
                         }
                     } else {
@@ -1413,8 +1401,6 @@ namespace iDynTree {
 
                 offset += group.second->stateJacobianBuffer.rows();
             }
-            m_pimpl->stateJacobianSparsity.resize(nonZeroIndexState); //remove leftovers
-
 
             stateSparsity = m_pimpl->stateJacobianSparsity;
 
@@ -1423,22 +1409,17 @@ namespace iDynTree {
 
         bool OptimalControlProblem::constraintsJacobianWRTControlSparsity(SparsityStructure &controlSparsity)
         {
-            size_t nonZeroIndexControl = 0;
             size_t offset = 0;
+            m_pimpl->controlJacobianSparsity.clear();
             for (auto& group : m_pimpl->constraintsGroups){
 
                 group.second->hasControlJacobianSparsity = group.second->group_ptr->constraintJacobianWRTControlSparsity(group.second->controlJacobianSparsity);
 
                 if (group.second->hasControlJacobianSparsity) {
 
-                    if (m_pimpl->controlJacobianSparsity.size() < (nonZeroIndexControl + group.second->controlJacobianSparsity.size())) {
-                        m_pimpl->controlJacobianSparsity.resize(nonZeroIndexControl + group.second->controlJacobianSparsity.size());
-                    }
-
                     for (size_t i = 0; i < group.second->controlJacobianSparsity.size(); ++i) {
-                        m_pimpl->controlJacobianSparsity.nonZeroElementRows[nonZeroIndexControl]    = group.second->controlJacobianSparsity.nonZeroElementRows[i] + offset;
-                        m_pimpl->controlJacobianSparsity.nonZeroElementColumns[nonZeroIndexControl] = group.second->controlJacobianSparsity.nonZeroElementColumns[i];
-                        nonZeroIndexControl++;
+                        m_pimpl->controlJacobianSparsity.add(group.second->controlJacobianSparsity[i].row + offset,
+                                                             group.second->controlJacobianSparsity[i].col);
                     }
 
                 } else {
@@ -1447,17 +1428,10 @@ namespace iDynTree {
 
                         size_t rows = group.second->controlJacobianBuffer.rows();
                         size_t cols = group.second->controlJacobianBuffer.cols();
-                        size_t nonZeros = rows * cols;
-
-                        if (m_pimpl->controlJacobianSparsity.size() < (nonZeroIndexControl + nonZeros)) {
-                            m_pimpl->controlJacobianSparsity.resize(nonZeroIndexControl + nonZeros);
-                        }
 
                         for (size_t i = 0; i < rows; ++i) {
                             for (size_t j = 0; j < cols; ++j) {
-                                m_pimpl->controlJacobianSparsity.nonZeroElementRows[nonZeroIndexControl]    = i + offset;
-                                m_pimpl->controlJacobianSparsity.nonZeroElementColumns[nonZeroIndexControl] = j;
-                                nonZeroIndexControl++;
+                                m_pimpl->controlJacobianSparsity.add(i + offset, j);
                             }
                         }
                     } else {
@@ -1470,8 +1444,6 @@ namespace iDynTree {
 
                 offset += group.second->controlJacobianBuffer.rows();
             }
-            m_pimpl->controlJacobianSparsity.resize(nonZeroIndexControl); //remove leftovers
-
 
             controlSparsity = m_pimpl->controlJacobianSparsity;
 
diff --git a/src/optimalcontrol/src/SparsityStructure.cpp b/src/optimalcontrol/src/SparsityStructure.cpp
index da7ac69dc62..671e4b00379 100644
--- a/src/optimalcontrol/src/SparsityStructure.cpp
+++ b/src/optimalcontrol/src/SparsityStructure.cpp
@@ -18,6 +18,13 @@
 #include <iDynTree/Core/Utils.h>
 #include <cassert>
 
+void iDynTree::optimalcontrol::SparsityStructure::addNonZero(size_t row, size_t col)
+{
+    m_nonZeroElementRows.push_back(row);
+    m_nonZeroElementColumns.push_back(col);
+    m_register.insert(std::to_string(row) + "_" + std::to_string(col));
+}
+
 iDynTree::optimalcontrol::SparsityStructure::SparsityStructure()
 { }
 
@@ -31,8 +38,8 @@ bool iDynTree::optimalcontrol::SparsityStructure::merge(const iDynTree::optimalc
         return false;
     }
 
-    for (size_t i = 0; i < other.nonZeroElementRows.size(); ++i) {
-        addNonZeroIfNotPresent(other.nonZeroElementRows[i], other.nonZeroElementColumns[i]);
+    for (size_t i = 0; i < other.size(); ++i) {
+        add(other.nonZeroElementRows()[i], other.nonZeroElementColumns()[i]);
     }
 
     return true;
@@ -42,7 +49,7 @@ void iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(size_t startRow,
 {
     for (size_t i = 0; i < numberOfRows; ++i) {
         for (size_t j = 0; j < numberOfColumns; ++j) {
-            addNonZeroIfNotPresent(startRow + i, startColumn + j);
+            add(startRow + i, startColumn + j);
         }
     }
 }
@@ -70,7 +77,7 @@ bool iDynTree::optimalcontrol::SparsityStructure::addDenseBlock(const iDynTree::
 void iDynTree::optimalcontrol::SparsityStructure::addIdentityBlock(size_t startRow, size_t startColumn, size_t dimension)
 {
     for (size_t i = 0; i < dimension; ++i) {
-        addNonZeroIfNotPresent(startRow + i, startColumn + i);
+        add(startRow + i, startColumn + i);
     }
 }
 
@@ -82,49 +89,70 @@ bool iDynTree::optimalcontrol::SparsityStructure::addBlock(size_t startRow, size
     }
 
     for (size_t i = 0; i < other.size(); ++i) {
-        addNonZeroIfNotPresent(startRow + other.nonZeroElementRows[i], startColumn + other.nonZeroElementColumns[i]);
+        add(startRow + other.nonZeroElementRows()[i], startColumn + other.nonZeroElementColumns()[i]);
     }
 
     return true;
 }
 
-void iDynTree::optimalcontrol::SparsityStructure::addNonZeroIfNotPresent(size_t newRow, size_t newCol)
+void iDynTree::optimalcontrol::SparsityStructure::add(size_t newRow, size_t newCol)
 {
     if (!isValuePresent(newRow, newCol)) {
-        nonZeroElementRows.push_back(newRow);
-        nonZeroElementColumns.push_back(newCol);
+        addNonZero(newRow, newCol);
     }
 }
 
-bool iDynTree::optimalcontrol::SparsityStructure::isValuePresent(size_t row, size_t col) const
+void iDynTree::optimalcontrol::SparsityStructure::add(NonZero newElement)
 {
-    for (size_t i = 0; i < nonZeroElementRows.size(); ++i) {
-        if ((row == nonZeroElementRows[i]) && (col == nonZeroElementColumns[i])) {
-            return true;
-        }
+    if (!isValuePresent(newElement.row, newElement.col)) {
+        addNonZero(newElement.row, newElement.col);
     }
-    return false;
 }
 
-void iDynTree::optimalcontrol::SparsityStructure::resize(size_t newSize)
+bool iDynTree::optimalcontrol::SparsityStructure::isValuePresent(size_t row, size_t col) const
+{
+    return (m_register.find(std::to_string(row) + "_" + std::to_string(col)) != m_register.end());
+}
+
+void iDynTree::optimalcontrol::SparsityStructure::reserve(size_t newSize)
 {
-    nonZeroElementRows.resize(newSize);
-    nonZeroElementColumns.resize(newSize);
+    m_nonZeroElementRows.reserve(newSize);
+    m_nonZeroElementColumns.reserve(newSize);
+    m_register.reserve(newSize);
 }
 
 void iDynTree::optimalcontrol::SparsityStructure::clear()
 {
-    nonZeroElementRows.clear();
-    nonZeroElementColumns.clear();
+    m_register.clear();
+    m_nonZeroElementRows.clear();
+    m_nonZeroElementColumns.clear();
 }
 
 size_t iDynTree::optimalcontrol::SparsityStructure::size() const
 {
     assert(isValid());
-    return nonZeroElementRows.size();
+    return m_nonZeroElementRows.size();
 }
 
 bool iDynTree::optimalcontrol::SparsityStructure::isValid() const
 {
-    return (nonZeroElementColumns.size() == nonZeroElementRows.size());
+    return (m_nonZeroElementColumns.size() == m_nonZeroElementRows.size());
+}
+
+iDynTree::optimalcontrol::NonZero iDynTree::optimalcontrol::SparsityStructure::operator[](size_t index) const
+{
+    NonZero element;
+    element.row = m_nonZeroElementRows[index];
+    element.col = m_nonZeroElementColumns[index];
+    return element;
+}
+
+const std::vector<size_t> &iDynTree::optimalcontrol::SparsityStructure::nonZeroElementRows() const
+{
+    return m_nonZeroElementRows;
+}
+
+const std::vector<size_t> &iDynTree::optimalcontrol::SparsityStructure::nonZeroElementColumns() const
+{
+    return m_nonZeroElementColumns;
 }
diff --git a/src/optimalcontrol/tests/MultipleShootingTest.cpp b/src/optimalcontrol/tests/MultipleShootingTest.cpp
index 2942d7f6d5f..f546922ba28 100644
--- a/src/optimalcontrol/tests/MultipleShootingTest.cpp
+++ b/src/optimalcontrol/tests/MultipleShootingTest.cpp
@@ -87,10 +87,10 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
 
     virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
         iDynTree::optimalcontrol::SparsityStructure sparsity;
-        sparsity.addNonZeroIfNotPresent(0, 0);
-        sparsity.addNonZeroIfNotPresent(0, 1);
-        sparsity.addNonZeroIfNotPresent(1, 0);
-        sparsity.addNonZeroIfNotPresent(1, 2);
+        sparsity.add(0, 0);
+        sparsity.add(0, 1);
+        sparsity.add(1, 0);
+        sparsity.add(1, 2);
         controlSparsity = sparsity;
         return true;
     }
@@ -194,7 +194,7 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
 
     virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
         iDynTree::optimalcontrol::SparsityStructure sparsity;
-        sparsity.addNonZeroIfNotPresent(0,0);
+        sparsity.add(0,0);
         controlSparsity = sparsity;
         return true;
     }
diff --git a/src/optimalcontrol/tests/OCProblemTest.cpp b/src/optimalcontrol/tests/OCProblemTest.cpp
index c4fd15141e9..8c384e09b00 100644
--- a/src/optimalcontrol/tests/OCProblemTest.cpp
+++ b/src/optimalcontrol/tests/OCProblemTest.cpp
@@ -83,10 +83,10 @@ class TestSystem : public iDynTree::optimalcontrol::DynamicalSystem {
 
     virtual bool dynamicsControlFirstDerivativeSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
         iDynTree::optimalcontrol::SparsityStructure sparsity;
-        sparsity.addNonZeroIfNotPresent(0, 0);
-        sparsity.addNonZeroIfNotPresent(0, 1);
-        sparsity.addNonZeroIfNotPresent(1, 0);
-        sparsity.addNonZeroIfNotPresent(1, 2);
+        sparsity.add(0, 0);
+        sparsity.add(0, 1);
+        sparsity.add(1, 0);
+        sparsity.add(1, 2);
         controlSparsity = sparsity;
         return true;
     }
@@ -163,7 +163,7 @@ class TestConstraint : public iDynTree::optimalcontrol::Constraint {
 
     virtual bool constraintJacobianWRTControlSparsity(iDynTree::optimalcontrol::SparsityStructure& controlSparsity) override {
         iDynTree::optimalcontrol::SparsityStructure sparsity;
-        sparsity.addNonZeroIfNotPresent(0,0);
+        sparsity.add(0,0);
         controlSparsity = sparsity;
         return true;
     }
@@ -401,16 +401,16 @@ int main() {
     controlZeroCheck.zero();
 
     for(size_t i = 0; i < stateSparsity.size(); ++i) {
-        unsigned int row = static_cast<unsigned int>(stateSparsity.nonZeroElementRows[i]);
-        unsigned int col = static_cast<unsigned int>(stateSparsity.nonZeroElementColumns[i]);
+        unsigned int row = static_cast<unsigned int>(stateSparsity[i].row);
+        unsigned int col = static_cast<unsigned int>(stateSparsity[i].col);
         stateSparsityCheck(row, col) = 0.0;
     }
 
     ASSERT_EQUAL_MATRIX(stateSparsityCheck, stateZeroCheck);
 
     for(size_t i = 0; i < controlSparsity.size(); ++i) {
-        unsigned int row = static_cast<unsigned int>(controlSparsity.nonZeroElementRows[i]);
-        unsigned int col = static_cast<unsigned int>(controlSparsity.nonZeroElementColumns[i]);
+        unsigned int row = static_cast<unsigned int>(controlSparsity[i].row);
+        unsigned int col = static_cast<unsigned int>(controlSparsity[i].col);
         controlSparsityCheck(row, col) = 0.0;
     }
 

From f775623f1647620b76ee9fd77f12be40f491b7aa Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 9 May 2019 22:23:34 +0200
Subject: [PATCH 113/194] Added bug when adding the sparisty of the transpose
 of a block.

---
 src/optimalcontrol/src/MultipleShootingSolver.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 94d2782aba4..4ed43fc7c77 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -285,7 +285,7 @@ namespace iDynTree {
                 SparsityStructure& transposeSparsity = m_hessianBlocks.blockSparsity(initCol, initRow);
 
                 for (size_t i = 0; i < sparsity.size(); ++i){
-                    transposeSparsity.add(sparsity[i]);
+                    transposeSparsity.add(sparsity[i].col, sparsity[i].row);
                 }
             }
 

From 95dab9463285a20fedcb1198af71d93c21df29ab Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 9 May 2019 23:14:26 +0200
Subject: [PATCH 114/194] Fixed bug in the sparsity of L2NormCost.

---
 src/optimalcontrol/src/L2NormCost.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/optimalcontrol/src/L2NormCost.cpp b/src/optimalcontrol/src/L2NormCost.cpp
index 9296fcd6fb0..9333381ff92 100644
--- a/src/optimalcontrol/src/L2NormCost.cpp
+++ b/src/optimalcontrol/src/L2NormCost.cpp
@@ -589,7 +589,7 @@ namespace iDynTree {
 
         bool L2NormCost::costSecondPartialDerivativeWRTControlSparsity(SparsityStructure &controlSparsity)
         {
-            controlSparsity = m_pimpl->stateCost.getHessianSparsity();
+            controlSparsity = m_pimpl->controlCost.getHessianSparsity();
             return true;
         }
 

From 816e146938fe557554005738a66c3d274987daed Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 13 May 2019 10:10:56 +0200
Subject: [PATCH 115/194] Ipopt is expecting only the lower triangular part of
 the hessian.

---
 src/optimalcontrol/src/IpoptInterface.cpp | 55 ++++++++++++++++-------
 1 file changed, 40 insertions(+), 15 deletions(-)

diff --git a/src/optimalcontrol/src/IpoptInterface.cpp b/src/optimalcontrol/src/IpoptInterface.cpp
index b6ccc038c03..2a887908111 100644
--- a/src/optimalcontrol/src/IpoptInterface.cpp
+++ b/src/optimalcontrol/src/IpoptInterface.cpp
@@ -55,7 +55,8 @@ namespace iDynTree {
         public:
 
             unsigned int numberOfVariables, numberOfConstraints;
-            std::vector<size_t> constraintsJacNNZRows, constraintsJacNNZCols, hessianNNZRows, hessianNNZCols;
+            std::vector<size_t> constraintsJacNNZRows, constraintsJacNNZCols, inputHessianNNZRows, inputHessianNNZCols,
+                lowerTriangularHessianNNZRows, lowerTriangularHessianNNZCols;
             std::shared_ptr<OptimizationProblem> problem;
             double minusInfinity, plusInfinity; //TODO. Set these before solving
             VectorDynSize solution;
@@ -117,7 +118,7 @@ namespace iDynTree {
 
                 nnz_jac_g = static_cast<Ipopt::Index>(constraintsJacNNZRows.size()); //set in the solve method
 
-                nnz_h_lag = static_cast<Ipopt::Index>(hessianNNZRows.size()); //set in the solve method
+                nnz_h_lag = static_cast<Ipopt::Index>(lowerTriangularHessianNNZRows.size());
 
                 index_style = C_STYLE;
                 return true;
@@ -346,12 +347,13 @@ namespace iDynTree {
                     toEigen(m_lagrangianHessianBuffer) = obj_factor * toEigen(m_costHessianBuffer) + toEigen(m_constraintsHessianBuffer);
                 }
 
-                for (size_t i = 0; i < hessianNNZRows.size(); ++i){
+                for (size_t i = 0; i < lowerTriangularHessianNNZRows.size(); ++i){
                     if (values == nullptr){
-                        iRow[i] = static_cast<Ipopt::Index>(hessianNNZRows[i]); //these two vectors have been filled in the get_nlp_info method
-                        jCol[i] = static_cast<Ipopt::Index>(hessianNNZCols[i]);
+                        iRow[i] = static_cast<Ipopt::Index>(lowerTriangularHessianNNZRows[i]); //these two vectors have been filled in the get_nlp_info method
+                        jCol[i] = static_cast<Ipopt::Index>(lowerTriangularHessianNNZCols[i]);
                     } else {
-                        values[i] = m_lagrangianHessianBuffer(static_cast<unsigned int>(hessianNNZRows[i]), static_cast<unsigned int>(hessianNNZCols[i]));
+                        values[i] = m_lagrangianHessianBuffer(static_cast<unsigned int>(lowerTriangularHessianNNZRows[i]),
+                                                              static_cast<unsigned int>(lowerTriangularHessianNNZCols[i]));
                     }
                 }
 
@@ -480,7 +482,7 @@ namespace iDynTree {
                 bool sameVariables = (previousNumberOfVariables == nlpPointer->numberOfVariables);
                 bool sameConstraints = (previousNumberOfConstraints == nlpPointer->numberOfConstraints);
                 bool sameNNZJac = (previousJacobianNonZeros == nlpPointer->constraintsJacNNZRows.size());
-                bool sameNNZHes = (previousHessianNonZeros == nlpPointer->hessianNNZRows.size());
+                bool sameNNZHes = (previousHessianNonZeros == nlpPointer->lowerTriangularHessianNNZRows.size());
                 return alreadySolved && sameVariables && sameConstraints && sameNNZJac && sameNNZHes;
             }
         };
@@ -565,18 +567,41 @@ namespace iDynTree {
             }
 
             if (m_problem->info().hasSparseHessian()) {
-                if (!(m_problem->getHessianInfo(m_pimpl->nlpPointer->hessianNNZRows,
-                                                m_pimpl->nlpPointer->hessianNNZCols))){
+                if (!(m_problem->getHessianInfo(m_pimpl->nlpPointer->inputHessianNNZRows,
+                                                m_pimpl->nlpPointer->inputHessianNNZCols))){
                     reportError("IpoptInterface", "solve", "Error while retrieving hessian info.");
                     return false;
                 }
+
+                std::vector<size_t>& iRows = m_pimpl->nlpPointer->inputHessianNNZRows;
+                std::vector<size_t>& jCols = m_pimpl->nlpPointer->inputHessianNNZCols;
+
+                std::vector<size_t>& iRowsLT = m_pimpl->nlpPointer->lowerTriangularHessianNNZRows;
+                std::vector<size_t>& jColsLT = m_pimpl->nlpPointer->lowerTriangularHessianNNZCols;
+
+                size_t nnz = 0;
+                for (size_t i = 0; i < iRows.size(); ++i) {
+                    if (jCols[i] <= iRows[i]) { //taking only the lower triangular part
+                        if (nnz < iRowsLT.size()) {
+                            iRowsLT[nnz] = iRows[i];
+                            jColsLT[nnz] = jCols[i];
+                        } else {
+                            iRowsLT.push_back(iRows[i]);
+                            jColsLT.push_back(jCols[i]);
+                        }
+                        ++nnz;
+                    }
+                }
+                iRowsLT.resize(nnz);
+                jColsLT.resize(nnz);
+
             } else { //dense hessian
-                m_pimpl->nlpPointer->hessianNNZRows.clear();
-                m_pimpl->nlpPointer->hessianNNZCols.clear();
+                m_pimpl->nlpPointer->lowerTriangularHessianNNZRows.clear();
+                m_pimpl->nlpPointer->lowerTriangularHessianNNZCols.clear();
                 for (unsigned int i = 0; i < m_pimpl->nlpPointer->numberOfVariables; i++) {
-                    for (unsigned int j = 0; j < m_pimpl->nlpPointer->numberOfVariables; ++j) {
-                        m_pimpl->nlpPointer->hessianNNZRows.push_back(i);
-                        m_pimpl->nlpPointer->hessianNNZCols.push_back(j);
+                    for (unsigned int j = 0; j <= i; ++j) {
+                        m_pimpl->nlpPointer->lowerTriangularHessianNNZRows.push_back(i);
+                        m_pimpl->nlpPointer->lowerTriangularHessianNNZCols.push_back(j);
                     }
                 }
             }
@@ -604,7 +629,7 @@ namespace iDynTree {
             m_pimpl->previousNumberOfVariables = m_pimpl->nlpPointer->numberOfVariables;
             m_pimpl->previousNumberOfConstraints = m_pimpl->nlpPointer->numberOfConstraints;
             m_pimpl->previousJacobianNonZeros = m_pimpl->nlpPointer->constraintsJacNNZRows.size();
-            m_pimpl->previousHessianNonZeros = m_pimpl->nlpPointer->hessianNNZRows.size();
+            m_pimpl->previousHessianNonZeros = m_pimpl->nlpPointer->inputHessianNNZRows.size();
 
             return true;
         }

From f29db20bebab66f4faa84de529bcc46a57bcf699 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 13 May 2019 15:08:37 +0200
Subject: [PATCH 116/194] Added possibility to specify regularization term for
 hessians in MultipleShootingSolver.

---
 .../OCSolvers/MultipleShootingSolver.h        |  7 +++
 .../src/MultipleShootingSolver.cpp            | 62 +++++++++++++++++++
 2 files changed, 69 insertions(+)

diff --git a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
index 01728130666..ec0ce6b6ac7 100644
--- a/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
+++ b/src/optimalcontrol/include/iDynTree/OCSolvers/MultipleShootingSolver.h
@@ -76,6 +76,13 @@ namespace iDynTree {
 
             void resetTranscription();
 
+            void addConstraintsHessianRegularization(double regularization);
+
+            void disableConstraintsHessianRegularization();
+
+            void addCostsHessianRegularization(double regularization);
+
+            void disableCostsHessianRegularization();
 
         private:
 
diff --git a/src/optimalcontrol/src/MultipleShootingSolver.cpp b/src/optimalcontrol/src/MultipleShootingSolver.cpp
index 4ed43fc7c77..679f9039f6d 100644
--- a/src/optimalcontrol/src/MultipleShootingSolver.cpp
+++ b/src/optimalcontrol/src/MultipleShootingSolver.cpp
@@ -174,6 +174,9 @@ namespace iDynTree {
             CollocationHessianSparsityMap m_systemStateHessianSparsity, m_systemControlHessianSparsity, m_systemMixedHessianSparsity;
             SparsityStructure m_fullHessianSparsity;
 
+            bool m_useCostRegularization, m_useConstraintsRegularization;
+            double m_constraintsRegularization, m_costsRegularization;
+
 
             friend class MultipleShootingSolver;
 
@@ -1044,6 +1047,10 @@ namespace iDynTree {
             , m_stateGuesses(nullptr)
             , m_controlGuesses(nullptr)
             , m_solved(false)
+            , m_useCostRegularization(false)
+            , m_useConstraintsRegularization(false)
+            , m_constraintsRegularization(0.0)
+            , m_costsRegularization(0.0)
             { }
 
             MultipleShootingTranscription(const std::shared_ptr<OptimalControlProblem> problem, const std::shared_ptr<Integrator> integrationMethod)
@@ -1066,6 +1073,10 @@ namespace iDynTree {
             , m_stateGuesses(nullptr)
             , m_controlGuesses(nullptr)
             , m_solved(false)
+            , m_useCostRegularization(false)
+            , m_useConstraintsRegularization(false)
+            , m_constraintsRegularization(0.0)
+            , m_costsRegularization(0.0)
             { }
 
             MultipleShootingTranscription(const MultipleShootingTranscription& other) = delete;
@@ -1427,6 +1438,10 @@ namespace iDynTree {
 
                 m_hessianBlocks.getFullSparsity(m_fullHessianSparsity);
 
+                if (m_useCostRegularization || m_useConstraintsRegularization) {
+                    m_fullHessianSparsity.addIdentityBlock(0, 0, m_numberOfVariables);
+                }
+
                 m_prepared = true;
                 return true;
             }
@@ -1817,6 +1832,12 @@ namespace iDynTree {
 
                 iDynTreeEigenMatrixMap hessianMap = toEigen(hessian);
 
+                if (m_useCostRegularization) {
+                    for (unsigned int i = 0; i < hessian.rows(); ++i) {
+                        hessian(i, i) = 0.0;
+                    }
+                }
+
                 for (auto mesh = m_meshPoints.begin(); mesh != m_meshPointsEnd; ++mesh){
 
                     stateBufferMap = variablesBuffer.segment(static_cast<Eigen::Index>(mesh->stateIndex), nx);
@@ -1855,6 +1876,13 @@ namespace iDynTree {
                         hessianMap.block(static_cast<Eigen::Index>(mesh->controlIndex), static_cast<Eigen::Index>(mesh->controlIndex), nu, nu) += costControlHessian;
                     }
                 }
+
+                if (m_useCostRegularization) {
+                    for (unsigned int i = 0; i < hessian.rows(); ++i) {
+                        hessian(i, i) += m_costsRegularization;
+                    }
+                }
+
                 return true;
             }
 
@@ -2054,6 +2082,12 @@ namespace iDynTree {
                     hessian.resize(static_cast<unsigned int>(numberOfVariables()), static_cast<unsigned int>(numberOfVariables()));
                 }
 
+                if (m_useConstraintsRegularization) {
+                    for (unsigned int i = 0; i < hessian.rows(); ++i) {
+                        hessian(i, i) = 0.0;
+                    }
+                }
+
                 m_hessianBlocks.reset();
 
                 MeshPointOrigin first = MeshPointOrigin::FirstPoint();
@@ -2148,6 +2182,12 @@ namespace iDynTree {
                 }
                 assert(static_cast<size_t>(constraintIndex) == m_numberOfConstraints);
 
+                if (m_useConstraintsRegularization) {
+                    for (unsigned int i = 0; i < hessian.rows(); ++i) {
+                        hessian(i, i) += m_constraintsRegularization;
+                    }
+                }
+
                 return true;
             }
         };
@@ -2277,5 +2317,27 @@ namespace iDynTree {
         {
             m_transcription->reset();
         }
+
+        void iDynTree::optimalcontrol::MultipleShootingSolver::disableCostsHessianRegularization()
+        {
+            m_transcription->m_useCostRegularization = false;
+        }
+
+        void iDynTree::optimalcontrol::MultipleShootingSolver::disableConstraintsHessianRegularization()
+        {
+            m_transcription->m_useConstraintsRegularization = false;
+        }
+
+        void iDynTree::optimalcontrol::MultipleShootingSolver::addCostsHessianRegularization(double regularization)
+        {
+            m_transcription->m_useCostRegularization = true;
+            m_transcription->m_costsRegularization = regularization;
+        }
+
+        void iDynTree::optimalcontrol::MultipleShootingSolver::addConstraintsHessianRegularization(double regularization)
+        {
+            m_transcription->m_useConstraintsRegularization = true;
+            m_transcription->m_constraintsRegularization = regularization;
+        }
     }
 }

From d49599b0a859e2654bc48aaf75f67768404c02b0 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 16 May 2019 09:55:35 +0200
Subject: [PATCH 117/194] Updated Release notes.

---
 doc/releases/v0_12.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index e6183cda32d..7da969019e9 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -26,7 +26,8 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Fixed bugs in ``MultipleShooting`` solver.
 * Added few lines of documentation.
 * Added interface for ``ALGLIB`` and ``WORHP``.
-* Taking into account also the sparsity pattern of constraints and dynamical system.
+* Multiple shooting solvers can use hessians of costs and constraints
+* Taking into account also the sparsity pattern of constraints and dynamical system (both in jacobians and hessians).
 
 #### `visualization`
 * Added visualization of vectors.

From 378f054927f9a6811bf71e5ecb442c778bbfc1f1 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 16 May 2019 14:06:30 +0200
Subject: [PATCH 118/194] Added missing header in SparsityStructure.

---
 src/optimalcontrol/include/iDynTree/SparsityStructure.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/optimalcontrol/include/iDynTree/SparsityStructure.h b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
index 15097d55350..bf41abd6e0b 100644
--- a/src/optimalcontrol/include/iDynTree/SparsityStructure.h
+++ b/src/optimalcontrol/include/iDynTree/SparsityStructure.h
@@ -21,6 +21,7 @@
 #include <vector>
 #include <cstddef>
 #include <unordered_set>
+#include <string>
 
 namespace iDynTree {
     namespace optimalcontrol {

From 16907fcf6efa39f61034696c49442f4978945f93 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Wed, 29 May 2019 15:27:08 +0200
Subject: [PATCH 119/194] implement Model::getTotalMass()

---
 src/model/include/iDynTree/Model/Model.h |  5 +++++
 src/model/src/Model.cpp                  | 12 ++++++++++++
 2 files changed, 17 insertions(+)

diff --git a/src/model/include/iDynTree/Model/Model.h b/src/model/include/iDynTree/Model/Model.h
index f14fa6c20b8..7a07538de33 100644
--- a/src/model/include/iDynTree/Model/Model.h
+++ b/src/model/include/iDynTree/Model/Model.h
@@ -217,6 +217,11 @@ namespace iDynTree
          */
         std::string getJointName(const JointIndex index) const;
 
+        /**
+         * Get the total mass of the robot
+         */
+        double getTotalMass() const;
+
         /**
          * Get the index of a joint, given a jointName.
          * If the jointName is not found in the model,
diff --git a/src/model/src/Model.cpp b/src/model/src/Model.cpp
index 4437f5af9ea..e1cb29852b3 100644
--- a/src/model/src/Model.cpp
+++ b/src/model/src/Model.cpp
@@ -132,6 +132,18 @@ LinkIndex Model::getLinkIndex(const std::string& linkName) const
     return LINK_INVALID_INDEX;
 }
 
+double Model::getTotalMass() const
+{
+    double totalMass = 0.0;
+
+    for(size_t l=0; l < this->getNrOfLinks(); l++)
+    {
+        totalMass += this->getLink(l)->getInertia().getMass();
+    }
+
+    return totalMass;
+}
+
 bool Model::isValidLinkIndex(const LinkIndex index) const
 {
     return (index != LINK_INVALID_INDEX) &&

From 49726403a05b902e2920b9c0d56c335a01b0c34e Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Wed, 29 May 2019 17:25:37 +0200
Subject: [PATCH 120/194] update release note

---
 doc/releases/v0_12.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 7da969019e9..37746e594db 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -50,3 +50,6 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)
 * Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for `Span` (https://github.com/robotology/idyntree/pull/522)
+
+### `Model`
+* Implement `getTotalMass()` method for Model class

From 4bc4f362a8cd84364cd4fc649b34d4a1637bbca5 Mon Sep 17 00:00:00 2001
From: Lorenzo Rapetti <lor.rapetti@gmail.com>
Date: Tue, 11 Jun 2019 14:54:15 +0200
Subject: [PATCH 121/194] Improved robot state publisher (#538)

---
 doc/releases/v0_12.md                         |  4 ++
 .../yarprobotstatepublisher/CMakeLists.txt    | 25 ++++---
 .../yarprobotstatepublisher/JointState.msg    |  6 --
 .../include/robotstatepublisher.h             | 18 ++---
 .../yarprobotstatepublisher/src/main.cpp      |  5 +-
 .../src/robotstatepublisher.cpp               | 65 ++++++++++++-------
 6 files changed, 73 insertions(+), 50 deletions(-)
 delete mode 100644 src/tools/yarprobotstatepublisher/JointState.msg

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 37746e594db..1718408ff67 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -53,3 +53,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 
 ### `Model`
 * Implement `getTotalMass()` method for Model class
+
+### `yarprobotstatepublisher`
+* Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
+* `robot` option is deprecated, and replaced by `name-prefix`.
diff --git a/src/tools/yarprobotstatepublisher/CMakeLists.txt b/src/tools/yarprobotstatepublisher/CMakeLists.txt
index ec47729d2dc..39f0f045bb7 100644
--- a/src/tools/yarprobotstatepublisher/CMakeLists.txt
+++ b/src/tools/yarprobotstatepublisher/CMakeLists.txt
@@ -1,17 +1,22 @@
-# Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia (IIT)  
+# Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia (IIT)
 # All Rights Reserved.
 # Authors: Silvio Traversaro <silvio.traversaro@iit.it>
 
 project(yarprobotstatepublisher)
 
-set(ROS_IDL_FILES JointState.msg)
-yarp_add_idl(IDL_GEN_FILES ${ROS_IDL_FILES})
-message("IDL_GEN_FILES : " ${IDL_GEN_FILES})
-add_executable(${PROJECT_NAME} include/robotstatepublisher.h src/main.cpp src/robotstatepublisher.cpp ${IDL_GEN_FILES})
-target_include_directories(${PROJECT_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/include
-                                                     ${YARP_INCLUDE_DIRS})
-# Workaround for yarp_add_idl problems
-target_include_directories(${PROJECT_NAME} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+find_package(YARP COMPONENTS rosmsg REQUIRED)
+
+add_executable(${PROJECT_NAME}
+    include/robotstatepublisher.h
+    src/main.cpp
+    src/robotstatepublisher.cpp)
+
+target_include_directories(${PROJECT_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/include)
+
+target_link_libraries(${PROJECT_NAME}
+    ${YARP_LIBRARIES}
+    YARP::YARP_rosmsg
+    idyntree-high-level
+    idyntree-yarp)
 
-target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES} idyntree-high-level idyntree-yarp)
 install(TARGETS ${PROJECT_NAME} DESTINATION bin)
diff --git a/src/tools/yarprobotstatepublisher/JointState.msg b/src/tools/yarprobotstatepublisher/JointState.msg
deleted file mode 100644
index 403b65cb1de..00000000000
--- a/src/tools/yarprobotstatepublisher/JointState.msg
+++ /dev/null
@@ -1,6 +0,0 @@
-Header header
-
-string[] name
-float64[] position
-float64[] velocity
-float64[] effort
diff --git a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
index 147c9f364ce..ef2d5b2e12f 100644
--- a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
+++ b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
@@ -22,21 +22,21 @@
 #include <iDynTree/Core/VectorDynSize.h>
 #include <iDynTree/KinDynComputations.h>
 
-#include <JointState.h>
+#include <yarp/rosmsg/sensor_msgs/JointState.h>
 
 class YARPRobotStatePublisherModule;
 
 /****************************************************************/
-class JointStateSuscriber: public yarp::os::Subscriber<JointState>
+class JointStateSubscriber: public yarp::os::Subscriber<yarp::rosmsg::sensor_msgs::JointState>
 {
 private:
     YARPRobotStatePublisherModule* m_module;
 
 public:
-    JointStateSuscriber();
+    JointStateSubscriber();
     void attach(YARPRobotStatePublisherModule* module);
-    using yarp::os::Subscriber<JointState>::onRead;
-    virtual void        onRead(JointState &v);
+    using yarp::os::Subscriber<yarp::rosmsg::sensor_msgs::JointState>::onRead;
+    virtual void        onRead(yarp::rosmsg::sensor_msgs::JointState &v);
 };
 
 
@@ -47,6 +47,8 @@ class YARPRobotStatePublisherModule : public yarp::os::RFModule
     yarp::dev::PolyDriver       m_ddtransformclient;
     yarp::dev::IFrameTransform       *m_iframetrans;
 
+    std::string m_tfPrefix;
+
     // Clock-related workaround
     bool m_usingNetworkClock;
     yarp::os::NetworkClock m_netClock;
@@ -62,8 +64,8 @@ class YARPRobotStatePublisherModule : public yarp::os::RFModule
    yarp::os::Mutex m_mutex;
 
    // /JointState topic scruscriber
-   yarp::os::Node*      m_rosNode;
-   JointStateSuscriber* m_jointStateSubscriber;
+   std::unique_ptr<yarp::os::Node> m_rosNode;
+   std::unique_ptr<JointStateSubscriber> m_jointStateSubscriber;
 
 public:
     YARPRobotStatePublisherModule();
@@ -71,7 +73,7 @@ class YARPRobotStatePublisherModule : public yarp::os::RFModule
     bool close();
     double getPeriod();
     bool updateModule();
-    virtual void        onRead(JointState &v);
+    virtual void        onRead(yarp::rosmsg::sensor_msgs::JointState &v);
 };
 
 #endif
diff --git a/src/tools/yarprobotstatepublisher/src/main.cpp b/src/tools/yarprobotstatepublisher/src/main.cpp
index 755cf3e3547..a310e33dcc1 100644
--- a/src/tools/yarprobotstatepublisher/src/main.cpp
+++ b/src/tools/yarprobotstatepublisher/src/main.cpp
@@ -35,9 +35,11 @@ int main(int argc, char *argv[])
     if (rf.check("help"))
     {
         cout<<"Options"<<endl;
-        cout<<"\t--robot                <robot-name>: robot name"<<endl;
+        cout<<"\t--name-prefix            <name-prefix>: prefix of the yarprobotstatepublisher ports (default no prefix)"<<endl;
+        cout<<"\t--tf-prefix              <tf-prefix>: prefix of the published TFs (default no prefix)"<<endl;
         cout<<"\t--model                  <file-name>: file name of the model to load at startup"<<endl;
         cout<<"\t--base-frame             <frame-name>: specify the base frame of the published tf tree"<<endl;
+        cout<<"\t--jointstates-topic      <topic-name>: source ROS topic that streams the joint state (default /joint_states)"<<endl;
         return EXIT_SUCCESS;
     }
 
@@ -50,4 +52,3 @@ int main(int argc, char *argv[])
     YARPRobotStatePublisherModule statepublisher;
     return statepublisher.runModule(rf);
 }
-
diff --git a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
index 39c019677a1..ff32bbb1513 100644
--- a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
+++ b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
@@ -19,7 +19,7 @@
 
 #include <yarp/math/Math.h>
 
-#include <iDynTree/Model/Model.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
 #include <iDynTree/KinDynComputations.h>
 #include <iDynTree/yarp/YARPConversions.h>
 
@@ -32,18 +32,18 @@ using namespace yarp::sig;
 using namespace yarp::math;
 
 /************************************************************/
-JointStateSuscriber::JointStateSuscriber(): m_module(nullptr)
+JointStateSubscriber::JointStateSubscriber(): m_module(nullptr)
 {
 }
 
 /************************************************************/
-void JointStateSuscriber::attach(YARPRobotStatePublisherModule* module)
+void JointStateSubscriber::attach(YARPRobotStatePublisherModule* module)
 {
     m_module = module;
 }
 
 /************************************************************/
-void JointStateSuscriber::onRead(JointState& v)
+void JointStateSubscriber::onRead(yarp::rosmsg::sensor_msgs::JointState& v)
 {
     m_module->onRead(v);
 }
@@ -62,24 +62,42 @@ YARPRobotStatePublisherModule::YARPRobotStatePublisherModule(): m_iframetrans(nu
 bool YARPRobotStatePublisherModule::configure(ResourceFinder &rf)
 {
     string name="yarprobotstatepublisher";
-    m_rosNode = new yarp::os::Node("/yarprobotstatepublisher");
-    string robot=rf.check("robot",Value("isaacSim")).asString();
+    string namePrefix = rf.check("name-prefix",Value("")).asString();
+    string robot = rf.check("robot",Value("")).asString();
+    if (!namePrefix.empty()) {
+        m_rosNode.reset(new yarp::os::Node("/"+namePrefix+"/yarprobotstatepublisher"));
+    }
+    else if (!robot.empty()) {
+        m_rosNode.reset(new yarp::os::Node("/"+robot+"/yarprobotstatepublisher"));
+        std::cerr << "[WARNING] The yarprobotstatepublisher option robot is deprecated," << std::endl <<
+                     "[WARNING] use name-prefix option instead";
+    }
+    else {
+        m_rosNode.reset(new yarp::os::Node("/yarprobotstatepublisher"));
+    }
+
     string modelFileName=rf.check("model",Value("model.urdf")).asString();
     m_period=rf.check("period",Value(0.010)).asDouble();
 
     Property pTransformclient_cfg;
     pTransformclient_cfg.put("device", "transformClient");
-    pTransformclient_cfg.put("local", "/"+name+"/transformClient");
+    if (!namePrefix.empty()) {
+        pTransformclient_cfg.put("local", "/"+namePrefix+"/"+name+"/transformClient");
+    }
+    else pTransformclient_cfg.put("local", "/"+name+"/transformClient");
+
     pTransformclient_cfg.put("remote", "/transformServer");
 
+    m_tfPrefix = rf.check("tf-prefix",Value("")).asString();
+
     bool ok_client = m_ddtransformclient.open(pTransformclient_cfg);
     if (!ok_client)
     {
         yError()<<"Problem in opening the transformClient device";
+        yError()<<"Is the transformServer YARP device running?";
         close();
         return false;
     }
-
     if (!m_ddtransformclient.view(m_iframetrans))
     {
         yError()<<"IFrameTransform I/F is not implemented";
@@ -97,7 +115,9 @@ bool YARPRobotStatePublisherModule::configure(ResourceFinder &rf)
 
     // Open the model
     string pathToModel=rf.findFileByName(modelFileName);
-    bool ok = m_kinDynComp.loadRobotModelFromFile(pathToModel);
+    iDynTree::ModelLoader modelLoader;
+    bool ok = modelLoader.loadModelFromFile(pathToModel);
+    ok = ok && m_kinDynComp.loadRobotModel(modelLoader.model());
     if (!ok || !m_kinDynComp.isValid())
     {
         yError()<<"Impossible to load file " << pathToModel;
@@ -131,9 +151,10 @@ bool YARPRobotStatePublisherModule::configure(ResourceFinder &rf)
     }
 
     // Setup the topic and configureisValid the onRead callback
-    m_jointStateSubscriber = new JointStateSuscriber();
+    string jointStatesTopicName = rf.check("jointstates-topic",Value("/joint_states")).asString();
+    m_jointStateSubscriber.reset(new JointStateSubscriber());
     m_jointStateSubscriber->attach(this);
-    m_jointStateSubscriber->topic("/joint_states");
+    m_jointStateSubscriber->topic(jointStatesTopicName);
     m_jointStateSubscriber->useCallback();
 
     return true;
@@ -150,7 +171,6 @@ bool YARPRobotStatePublisherModule::close()
     {
         m_jointStateSubscriber->interrupt();
         m_jointStateSubscriber->close();
-        delete m_jointStateSubscriber;
     }
 
     if (m_ddtransformclient.isValid())
@@ -162,10 +182,6 @@ bool YARPRobotStatePublisherModule::close()
 
     m_baseFrameIndex = iDynTree::FRAME_INVALID_INDEX;
 
-    if(m_rosNode)
-        delete m_rosNode;
-    m_rosNode = nullptr;
-
     return true;
 }
 
@@ -186,7 +202,7 @@ bool YARPRobotStatePublisherModule::updateModule()
 }
 
 /************************************************************/
-void YARPRobotStatePublisherModule::onRead(JointState &v)
+void YARPRobotStatePublisherModule::onRead(yarp::rosmsg::sensor_msgs::JointState &v)
 {
     yarp::os::LockGuard guard(m_mutex);
 
@@ -196,8 +212,8 @@ void YARPRobotStatePublisherModule::onRead(JointState &v)
         return;
     }
 
-    // Check size
-    if (v.name.size() != m_jointPos.size())
+    // Check if joint states contain at least as many joints of the model
+    if (v.name.size() < m_jointPos.size())
     {
         yError() << "Size mismatch. Model has " << m_jointPos.size()
                  << " joints, while the received JointState message has " << v.name.size() << " joints.";
@@ -209,10 +225,12 @@ void YARPRobotStatePublisherModule::onRead(JointState &v)
     //        * Add a map string --> indeces
     // Fill the buffer of joints positions
     const iDynTree::Model& model = m_kinDynComp.model();
+    iDynTree::JointIndex jntIndex;
     for (size_t i=0; i < v.name.size(); i++)
     {
-        iDynTree::JointIndex jntIndex = model.getJointIndex(v.name[i]);
-        if (jntIndex == iDynTree::JOINT_INVALID_INDEX)
+        jntIndex = model.getJointIndex(v.name[i]);
+
+        if (!(model.getJoint(jntIndex)->getNrOfDOFs()) || jntIndex == iDynTree::JOINT_INVALID_INDEX)
             continue;
 
         m_jointPos(model.getJoint(jntIndex)->getDOFsOffset()) = v.position[i];
@@ -221,7 +239,6 @@ void YARPRobotStatePublisherModule::onRead(JointState &v)
     // Set the updated joint positions
     m_kinDynComp.setJointPos(m_jointPos);
 
-    // Publish the frames on TF
     for (size_t frameIdx=0; frameIdx < model.getNrOfFrames(); frameIdx++)
     {
         if(m_baseFrameIndex == frameIdx)    // skip self-tranform
@@ -229,8 +246,8 @@ void YARPRobotStatePublisherModule::onRead(JointState &v)
 
         iDynTree::Transform base_H_frame = m_kinDynComp.getRelativeTransform(m_baseFrameIndex, frameIdx);
         iDynTree::toYarp(base_H_frame.asHomogeneousTransform(), m_buf4x4);
-        m_iframetrans->setTransform(model.getFrameName(frameIdx),
-                                    model.getFrameName(m_baseFrameIndex),
+        m_iframetrans->setTransform(m_tfPrefix + model.getFrameName(frameIdx),
+                                    m_tfPrefix + model.getFrameName(m_baseFrameIndex),
                                     m_buf4x4);
     }
 

From b91f83564dff465b34ad7032c3a29d291c8cb6cc Mon Sep 17 00:00:00 2001
From: Prashanth <prashanthramadoss@gmail.com>
Date: Thu, 13 Jun 2019 17:24:22 +0200
Subject: [PATCH 122/194] [tools][yarprobotstatepublisher] fix missing <memory>
 header

---
 src/tools/yarprobotstatepublisher/include/robotstatepublisher.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
index ef2d5b2e12f..784d6c58fad 100644
--- a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
+++ b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
@@ -23,6 +23,7 @@
 #include <iDynTree/KinDynComputations.h>
 
 #include <yarp/rosmsg/sensor_msgs/JointState.h>
+#include <memory>
 
 class YARPRobotStatePublisherModule;
 

From 516914554127c01cae48e86e576ee779a979350a Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 13 Jun 2019 17:30:16 +0200
Subject: [PATCH 123/194] Implement RPYRightTrivializedDerivativeRateOfChange()
 and RPYRightTrivializedDerivativeInverseRateOfChange() into Rotation class

---
 src/core/include/iDynTree/Core/Rotation.h | 21 +++++++++++
 src/core/src/Rotation.cpp                 | 45 +++++++++++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/src/core/include/iDynTree/Core/Rotation.h b/src/core/include/iDynTree/Core/Rotation.h
index cb85e6cec43..7fdf02ec962 100644
--- a/src/core/include/iDynTree/Core/Rotation.h
+++ b/src/core/include/iDynTree/Core/Rotation.h
@@ -365,6 +365,19 @@ namespace iDynTree
          */
         static Matrix3x3 RPYRightTrivializedDerivative(const double roll, const double pitch, const double yaw);
 
+        /**
+         * Return the rate of change of the right-trivialized derivative of the RPY function.
+         *
+         * If we indicate with \f$ rpy \in \mathbb{R}^3 \f$ the roll pitch yaw vector,
+         * and with \f$  RPY(rpy) : \mathbb{R}^3 \mapsto SO(3) \f$ the function implemented
+         * in the Rotation::RPY method, this method returns the right-trivialized partial
+         * derivative of Rotation::RPY, i.e. :
+         * \f[
+         *    (RPY(rpy) \frac{d}{d t}\frac{\partial RPY(rpy)}{\partial rpy})^\vee
+         * \f]
+         */
+        static Matrix3x3 RPYRightTrivializedDerivativeRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot);
+
         /**
          * Return the inverse of the right-trivialized derivative of the RPY function.
          *
@@ -373,6 +386,14 @@ namespace iDynTree
          */
         static Matrix3x3 RPYRightTrivializedDerivativeInverse(const double roll, const double pitch, const double yaw);
 
+        /**
+         * Return the inverse of the rate of change of right-trivialized derivative of the RPY function.
+         *
+         * See RPYRightTrivializedDerivativeRateOfChange for a detailed description of the method.
+         *
+         */
+        static Matrix3x3 RPYRightTrivializedDerivativeInverseRateOfChange(const double roll, const double pitch, const double yaw, const double rollDot, const double pitchDot, const double yawDot);
+
         /**
          * Return the right-trivialized derivative of the Quaternion function.
          *
diff --git a/src/core/src/Rotation.cpp b/src/core/src/Rotation.cpp
index 22fca8175a5..c434c451542 100644
--- a/src/core/src/Rotation.cpp
+++ b/src/core/src/Rotation.cpp
@@ -517,6 +517,28 @@ namespace iDynTree
         return map;
     }
 
+    Matrix3x3 Rotation::RPYRightTrivializedDerivativeRateOfChange(const double /*roll*/, const double pitch, const double yaw, const double /*rollDot*/, const double pitchDot, const double yawDot)
+    {
+        Matrix3x3 map;
+
+        double sp = std::sin(pitch);
+        double cp = std::cos(pitch);
+        double sy = std::sin(yaw);
+        double cy = std::cos(yaw);
+
+        map(0, 0) = -sp * cy * pitchDot - cp * sy * yawDot;
+        map(1, 0) = -sp * sy * pitchDot + cy * cp * yawDot;
+        map(2, 0) = -cp * pitchDot;
+        map(0, 1) = -cy * yawDot;
+        map(1, 1) = -sy * yawDot;
+        map(2, 1) = 0.0;
+        map(0, 2) = 0.0;
+        map(1, 2) = 0.0;
+        map(2, 2) = 0.0;
+
+        return map;
+    }
+
     Matrix3x3 Rotation::RPYRightTrivializedDerivativeInverse(const double /*roll*/, const double pitch, const double yaw)
     {
         // See doc/symbolic/RPYExpressionReference.py
@@ -541,6 +563,29 @@ namespace iDynTree
         return map;
     }
 
+    Matrix3x3 Rotation::RPYRightTrivializedDerivativeInverseRateOfChange(const double /*roll*/, const double pitch, const double yaw, const double /*rollDot*/, const double pitchDot, const double yawDot)
+    {
+        Matrix3x3 map;
+
+        double sp = std::sin(pitch);
+        double cp = std::cos(pitch);
+        double sy = std::sin(yaw);
+        double cy = std::cos(yaw);
+        double tp = std::tan(pitch);
+
+        map(0, 0) = (-sy * cp * yawDot + cy * sp * pitchDot) / std::pow(cp, 2);
+        map(1, 0) = -cy * yawDot;
+        map(2, 0) = -sy * tp * yawDot + cy * pitchDot / std::pow(cp, 2);
+        map(0, 1) = (cy * cp * yawDot + sy * sp * pitchDot) / std::pow(cp, 2);
+        map(1, 1) = -sy * yawDot;
+        map(2, 1) = cy * tp * yawDot + sy * pitchDot / std::pow(cp, 2);
+        map(0, 2) = 0.0;
+        map(1, 2) = 0.0;
+        map(2, 2) = 0.0;
+
+        return map;
+    }
+
     MatrixFixSize<4, 3> Rotation::QuaternionRightTrivializedDerivative(Vector4 quaternion)
     {
         MatrixFixSize<4, 3> outputMatrix;

From e4467f8625b1c68e8bf2281774dbdec8d7603ac6 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 13 Jun 2019 17:35:55 +0200
Subject: [PATCH 124/194] Implement unit test for
 RPYRightTrivializedDerivativeRateOfChange() and
 RPYRightTrivializedDerivativeInverseRateOfChange() methods

---
 src/core/tests/RotationUnitTest.cpp | 75 +++++++++++++++++++++++++++--
 1 file changed, 70 insertions(+), 5 deletions(-)

diff --git a/src/core/tests/RotationUnitTest.cpp b/src/core/tests/RotationUnitTest.cpp
index 1275c7b9647..9e7d7786f2b 100644
--- a/src/core/tests/RotationUnitTest.cpp
+++ b/src/core/tests/RotationUnitTest.cpp
@@ -63,6 +63,67 @@ void validateRPYRightTrivializedDerivative(const Rotation & R)
     }
 }
 
+void validateRPYRightTrivializedDerivativeRateOfChange(const Rotation & R, const Vector3 & eulerAngleRateOfChange)
+{
+    iDynTree::Vector3 rpy = R.asRPY();
+    Matrix3x3 DotRPYtoOmegaRateOfChange = Rotation::RPYRightTrivializedDerivativeRateOfChange(rpy(0),rpy(1),rpy(2),
+                                                                                              eulerAngleRateOfChange(0), eulerAngleRateOfChange(1),
+                                                                                              eulerAngleRateOfChange(2));
+
+    Matrix3x3 OmegaToDotRPYRateOfChange = Rotation::RPYRightTrivializedDerivativeInverseRateOfChange(rpy(0),rpy(1),rpy(2),
+                                                                                                     eulerAngleRateOfChange(0), eulerAngleRateOfChange(1),
+                                                                                                     eulerAngleRateOfChange(2));
+
+    Matrix3x3 OmegaToDotRPY = Rotation::RPYRightTrivializedDerivativeInverse(rpy(0),rpy(1),rpy(2));
+    Matrix3x3 DotRPYToOmega = Rotation::RPYRightTrivializedDerivative(rpy(0),rpy(1),rpy(2));
+
+    Matrix3x3 OmegaToDotRPYRateOfChangeCheck;
+    toEigen(OmegaToDotRPYRateOfChangeCheck) = -toEigen(OmegaToDotRPY) * toEigen(DotRPYtoOmegaRateOfChange) * toEigen(OmegaToDotRPY);
+
+    ASSERT_EQUAL_MATRIX(OmegaToDotRPYRateOfChangeCheck, OmegaToDotRPYRateOfChange);
+
+    Vector3 rpyRateOfChangePerturbedUp;
+    Vector3 rpyRateOfChangePerturbedDown;
+    Vector3 angularVelocity;
+    toEigen(angularVelocity) = toEigen(DotRPYToOmega) * toEigen(eulerAngleRateOfChange);
+
+    double numericalDerivStep = 1e-8;
+
+    Vector3 rpyUp;
+    Vector3 rpyDown;
+
+    toEigen(rpyUp) = toEigen(rpy) + toEigen(eulerAngleRateOfChange) * numericalDerivStep;
+    toEigen(rpyDown) = toEigen(rpy) - toEigen(eulerAngleRateOfChange) * numericalDerivStep;
+
+    Matrix3x3 DotRPYToOmegaUp = Rotation::RPYRightTrivializedDerivative(rpyUp(0),rpyUp(1),rpyUp(2));
+    Matrix3x3 DotRPYToOmegaDown = Rotation::RPYRightTrivializedDerivative(rpyDown(0),rpyDown(1),rpyDown(2));
+
+    for(size_t i=0; i < 3; i++)
+    {
+        rpyRateOfChangePerturbedUp = eulerAngleRateOfChange;
+        rpyRateOfChangePerturbedUp(i) = rpyRateOfChangePerturbedUp(i) + numericalDerivStep;
+        rpyRateOfChangePerturbedDown = eulerAngleRateOfChange;
+        rpyRateOfChangePerturbedDown(i) = rpyRateOfChangePerturbedDown(i) - numericalDerivStep;
+
+        Vector3 RPYNumSecondDev;
+        toEigen(RPYNumSecondDev) = (toEigen(rpyRateOfChangePerturbedUp) - toEigen(rpyRateOfChangePerturbedDown))/(2*numericalDerivStep);
+
+        Vector3 angularVelocityUp;
+        toEigen(angularVelocityUp) = toEigen(DotRPYToOmegaUp) * toEigen(rpyRateOfChangePerturbedUp);
+        Vector3 angularVelocityDown;
+        toEigen(angularVelocityDown) = toEigen(DotRPYToOmegaDown) * toEigen(rpyRateOfChangePerturbedDown);
+
+        Vector3 angularAccelerationNum;
+        toEigen(angularAccelerationNum) = (toEigen(angularVelocityUp) - toEigen(angularVelocityDown))/(2*numericalDerivStep);
+
+        Vector3 checkDoubleDotRPY;
+        toEigen(checkDoubleDotRPY) = toEigen(OmegaToDotRPYRateOfChange) * toEigen(angularVelocity)
+            + toEigen(OmegaToDotRPY) * toEigen(angularAccelerationNum);
+
+        ASSERT_EQUAL_VECTOR_TOL(checkDoubleDotRPY,RPYNumSecondDev,1e-4);
+    }
+}
+
 void validateQuaternionRightTrivializedDerivative(const Rotation & R)
 {
     double numericalDerivStep = 1e-8;
@@ -131,20 +192,24 @@ void validateQuaternionRightTrivializedDerivative(const Rotation & R)
     }
 }
 
-void validateAllTests(const Rotation & R)
+void validateAllTests(const Rotation & R, const Vector3 & eulerAngleRateOfChange)
 {
     validateRPYRightTrivializedDerivative(R);
     validateQuaternionRightTrivializedDerivative(R);
+    validateRPYRightTrivializedDerivativeRateOfChange(R, eulerAngleRateOfChange);
 }
 
 int main()
 {
-    // Check RPYRightTrivializedDerivative 
+    // Check RPYRightTrivializedDerivative
 
     Rotation R = iDynTree::getRandomRotation();
-    validateAllTests(R);
+    Vector3 eulerAngleRateOfChange;
+    iDynTree::getRandomVector(eulerAngleRateOfChange, -1.0, 1.0);
+    validateAllTests(R, eulerAngleRateOfChange);
     R = iDynTree::getRandomRotation();
-    validateAllTests(R);
-    
+    iDynTree::getRandomVector(eulerAngleRateOfChange, -1.0, 1.0);
+    validateAllTests(R,eulerAngleRateOfChange);
+
     return EXIT_SUCCESS;
 }

From aa2b237e0f2190659dbd3d3aa7aa25ce7701ec11 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 13 Jun 2019 17:38:35 +0200
Subject: [PATCH 125/194] update release changelog

---
 doc/releases/v0_12.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 1718408ff67..3370a7d4212 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -50,6 +50,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)
 * Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for `Span` (https://github.com/robotology/idyntree/pull/522)
+* Implement `RPYRightTrivializedDerivativeRateOfChange()` and `RPYRightTrivializedDerivativeInverseRateOfChange()` into `Rotation` class
 
 ### `Model`
 * Implement `getTotalMass()` method for Model class

From 4c56a691ebc60f5c0630e5a023de9f5245a2d579 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 13 Jun 2019 22:03:37 +0200
Subject: [PATCH 126/194] Fix comment in Rotation.h

---
 src/core/include/iDynTree/Core/Rotation.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/core/include/iDynTree/Core/Rotation.h b/src/core/include/iDynTree/Core/Rotation.h
index 7fdf02ec962..1021dce4f24 100644
--- a/src/core/include/iDynTree/Core/Rotation.h
+++ b/src/core/include/iDynTree/Core/Rotation.h
@@ -387,7 +387,7 @@ namespace iDynTree
         static Matrix3x3 RPYRightTrivializedDerivativeInverse(const double roll, const double pitch, const double yaw);
 
         /**
-         * Return the inverse of the rate of change of right-trivialized derivative of the RPY function.
+         *  Return the rate of change of the inverse of the right-trivialized derivative of the RPY function.
          *
          * See RPYRightTrivializedDerivativeRateOfChange for a detailed description of the method.
          *

From 6fe71e0a867be368d26e2ead773dcd8a5ebed15b Mon Sep 17 00:00:00 2001
From: Prashanth <prashanthramadoss@gmail.com>
Date: Wed, 10 Jul 2019 17:32:32 +0200
Subject: [PATCH 127/194] [AttitudeEstimator] fix quaternion propagation

---
 .../Estimation/AttitudeEstimatorUtils.h       | 44 +++++++++++++++++++
 .../Estimation/AttitudeQuaternionEKF.h        |  4 +-
 src/estimation/src/AttitudeEstimatorUtils.cpp |  4 +-
 src/estimation/src/AttitudeMahonyFilter.cpp   | 17 ++++---
 src/estimation/src/AttitudeQuaternionEKF.cpp  | 27 +++++++-----
 src/estimation/src/ExtendedKalmanFilter.cpp   |  4 +-
 .../tests/AttitudeEstimatorUnitTest.cpp       | 28 ++++++++++--
 7 files changed, 102 insertions(+), 26 deletions(-)

diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
index aea096314ef..e08e2dadf26 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeEstimatorUtils.h
@@ -147,4 +147,48 @@ iDynTree::UnitQuaternion composeQuaternion2(const iDynTree::UnitQuaternion &q1,
  */
 iDynTree::UnitQuaternion pureQuaternion(const iDynTree::Vector3& bodyFixedFrameVelocityInInertialFrame);
 
+
+/**
+ * @brief exponential map for quaternion - maps angular velocities to quaternion
+ *
+ * \f$ \text{exp}(\omega) = \begin{bmatrix} \text{cos}(\frac{||\omega||}{2}) \\ \text{sin}(\frac{||\omega||}{2})\frac{\omega}{||\omega||}  \end{bmatrix} \f$
+ *
+ * @param[in] omega angular velocity
+ * @return iDynTree::UnitQuaternion
+ */
+inline iDynTree::UnitQuaternion expQuaternion(iDynTree::Vector3 omega)
+{
+    iDynTree::UnitQuaternion q;
+    q.zero();
+    q(0) = 1.0;
+    using iDynTree::toEigen;
+    double norm{toEigen(omega).norm()};
+
+    if (norm == 0)
+    {
+        return q;
+    }
+
+    double c = std::cos(norm/2);
+    double s = std::sin(norm/2)/norm;
+
+    q(0) = c;
+    q(1) = omega(0)*s;
+    q(2) = omega(1)*s;
+    q(3) = omega(2)*s;
+
+    return q;
+}
+
+template<class T>
+inline bool check_are_almost_equal(const T& x, const T& y, int units_in_last_place)
+{
+    if (!( std::abs(x- y) <= std::numeric_limits<T>::epsilon()*std::max(std::abs(x), std::abs(y))*units_in_last_place))
+    {
+        return false;
+    }
+
+    return true;
+}
+
 #endif
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
index 15ef2a90a6c..c71eb154866 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
@@ -76,7 +76,7 @@ namespace iDynTree
      * @note: we will drop the subscripts and superscripts in the rest of the documentation for convenience
      *
      * Discretized dynamics during the prediction step,
-     * \f[ \hat{{x}}_{k+1} = \begin{bmatrix} q_k + \Delta t. \frac{1}{2}q_k  \circ \begin{bmatrix} 0 \\ \omega^b_{k+1} \end{bmatrix} \\ y_{gyro_{k}} - b_{k} \\ (1 - \lambda_{b} \Delta t)b_k \end{bmatrix} \f]
+     * \f[ \hat{{x}}_{k+1} = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro_{k}} - b_{k} \\ (1 - \lambda_{b} \Delta t)b_k \end{bmatrix} \f]
      *
      * Measurement model for accelerometer is given as,
      * \f[ h_{acc}(\hat{x}_{k+1}) = \begin{bmatrix} 2(q_1q_3 - q_0q_2) \\ 2(q_2q_3 - q_0q_1) \\ q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix} \f]
@@ -230,7 +230,7 @@ namespace iDynTree
          * discrete system propagation \f$ f(X, u) = f(X, y_gyro) \f$
          * where \f$ X = \begin{bmatrix} q_0 &  q_1 & q_2 & q_3 & \omega_x & \omega_y & \omega_z & \b_x & \b_y & \b_z \end{bmatrix}^T \f$
          * \f$ u = \begin{bmatrix} y_{gyro}_x & y_{gyro}_y & y_{gyro}_z \end{bmatrix}^T \f$
-         * \f$ f(X, u) = \begin{bmatrix} q + \Delta t. \frac{1}{2}q \circ \begin{bmatrix} 0 \\ \omega^b \end{bmatrix} \\ y_{gyro} - b \\ (1 - \lambda_{b} \Delta t)b \end{bmatrix}\f$
+         * \f$ f(X, u) = \begin{bmatrix} q_{k} \otimes \text{exp}(\omega \Delta T) \\ y_{gyro} - b \\ (1 - \lambda_{b} \Delta t)b \end{bmatrix}\f$
          */
         bool ekf_f(const iDynTree::VectorDynSize& x_k,
                const iDynTree::VectorDynSize& u_k,
diff --git a/src/estimation/src/AttitudeEstimatorUtils.cpp b/src/estimation/src/AttitudeEstimatorUtils.cpp
index 612c914cf0b..6030805e9d5 100644
--- a/src/estimation/src/AttitudeEstimatorUtils.cpp
+++ b/src/estimation/src/AttitudeEstimatorUtils.cpp
@@ -18,7 +18,7 @@ bool checkValidMeasurement(const iDynTree::Vector3& in, const std::string& measu
     {
         if (isZeroVector(in))
         {
-            iDynTree::reportError("AttitudeMahonyFilter", "checkValidMeasurement",
+            iDynTree::reportError("AttitudeEstimator", "checkValidMeasurement",
                                   (measurement_type + " measurements are invalid. Expecting a non-zero vector.").c_str());
             return false;
         }
@@ -26,7 +26,7 @@ bool checkValidMeasurement(const iDynTree::Vector3& in, const std::string& measu
 
     if (isVectorNaN(in))
     {
-        iDynTree::reportError("AttitudeMahonyFilter", "checkValidMeasurement",
+        iDynTree::reportError("AttitudeEstimator", "checkValidMeasurement",
                               (measurement_type + " measurements are invalid. Has NaN elements.").c_str());
         return false;
     }
diff --git a/src/estimation/src/AttitudeMahonyFilter.cpp b/src/estimation/src/AttitudeMahonyFilter.cpp
index 507949e0162..e16695153a6 100644
--- a/src/estimation/src/AttitudeMahonyFilter.cpp
+++ b/src/estimation/src/AttitudeMahonyFilter.cpp
@@ -116,18 +116,22 @@ bool iDynTree::AttitudeMahonyFilter::propagateStates()
     auto omega_mes(toEigen(m_omega_mes));
 
     // compute the correction from the measurements
-    gyroUpdate = Omega_y - b + (omega_mes*m_params_mahony.kp);
-    iDynTree::UnitQuaternion correction = pureQuaternion(gyro_update_dyn);
-    auto dq(toEigen(composeQuaternion2(m_state_mahony.m_orientation, correction)));
+    gyroUpdate = (Omega_y - b + (omega_mes*m_params_mahony.kp))*m_params_mahony.time_step_in_seconds*0.5;
+    iDynTree::UnitQuaternion correction = expQuaternion(gyro_update_dyn);
 
     // system dynamics equations
-    q = q + (dq*(m_params_mahony.time_step_in_seconds*0.5));
-    if (q.norm() == 0)
+    q = toEigen(composeQuaternion2(m_state_mahony.m_orientation, correction));
+
+    int precision{4};
+    double malformed_unit_quaternion_norm{0.0};
+    if (check_are_almost_equal(q.norm(), malformed_unit_quaternion_norm, precision))
     {
         reportError("AttitudeMahonyFilter", "propagateStates", "invalid quaternion with zero norm");
         return false;
     }
-    if (q.norm() != 1)
+
+    double unit_quaternion_norm{1.0};
+    if (!check_are_almost_equal(q.norm(), unit_quaternion_norm, precision))
     {
         q.normalize();
     }
@@ -136,6 +140,7 @@ bool iDynTree::AttitudeMahonyFilter::propagateStates()
 
     m_orientationInSO3 = iDynTree::Rotation::RotationFromQuaternion(m_state_mahony.m_orientation);
     m_orientationInRPY = iDynTree::Rotation::RotationFromQuaternion(m_state_mahony.m_orientation).asRPY();
+
     return true;
 }
 
diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp
index a70c97af6cd..0926f987782 100644
--- a/src/estimation/src/AttitudeQuaternionEKF.cpp
+++ b/src/estimation/src/AttitudeQuaternionEKF.cpp
@@ -152,13 +152,13 @@ bool iDynTree::AttitudeQuaternionEKF::initializeFilter()
     }
 
     if (!setSystemNoiseVariance(m_params_qekf.gyroscope_noise_variance,
-                            m_params_qekf.gyro_bias_noise_variance))
+                                m_params_qekf.gyro_bias_noise_variance))
     {
         return false;
     }
 
     if (!setMeasurementNoiseVariance(m_params_qekf.accelerometer_noise_variance,
-                                m_params_qekf.magnetometer_noise_variance))
+                                     m_params_qekf.magnetometer_noise_variance))
     {
         return false;
     }
@@ -267,7 +267,7 @@ bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTre
     m_A = A_R_B * m_yB;
     m_A(2) = 0; // to limit the vertical influence of magnetometer {^A}m_z = 0
     m_B_modified = A_R_B.transpose()* m_A; // rotate vector back to body frame
-    m_Mag_y = atan2(-m_B_modified(1), m_B_modified(0));
+    m_Mag_y = std::atan2(-m_B_modified(1), m_B_modified(0));
 
     // set accelerometer and magnetometer measurement
     if (!callEkfUpdate())
@@ -442,21 +442,26 @@ bool iDynTree::AttitudeQuaternionEKF::ekf_f(const iDynTree::VectorDynSize& x_k,
     auto x_hat_plus(toEigen(xhat_k_plus_one));
 
     q = x.block<4,1>(0, 0);
-    Omega = x.block<3,1>(4, 0);
+    Omega = x.block<3,1>(4, 0)*m_params_qekf.time_step_in_seconds;
     b = x.block<3,1>(7, 0);
 
-    iDynTree::UnitQuaternion correction = pureQuaternion(ang_vel);
-    auto dq(toEigen(composeQuaternion2(orientation, correction)));
+    iDynTree::UnitQuaternion correction = expQuaternion(ang_vel);
+    iDynTree::UnitQuaternion q_hat_plus = composeQuaternion2(orientation, correction);
 
-    x_hat_plus.block<4,1>(0, 0) = q + (dq*(m_params_qekf.time_step_in_seconds*0.5));
-    if (x_hat_plus.block<4,1>(0, 0).norm() == 0)
+    x_hat_plus.block<4,1>(0, 0) = toEigen(q_hat_plus);
+    double quat_norm = x_hat_plus.block<4,1>(0, 0).norm();
+
+    double malformed_unit_quaternion_norm{0.0};
+    if (check_are_almost_equal(quat_norm, malformed_unit_quaternion_norm, 4))
     {
         reportError("AttitudeQuaternionEKF", "ekf_f", "invalid quaternion");
         return false;
     }
-    if (q.norm() != 1)
+
+    double unit_quaternion_norm{1.0};
+    if (!check_are_almost_equal(quat_norm, unit_quaternion_norm, 4))
     {
-        q.normalize();
+        x_hat_plus.block<4,1>(0, 0).normalize();
     }
     x_hat_plus.block<3,1>(4, 0) = u - b;
     x_hat_plus.block<3,1>(7, 0) = b*(1 - (m_params_qekf.bias_correlation_time_factor*m_params_qekf.time_step_in_seconds));
@@ -512,7 +517,7 @@ bool iDynTree::AttitudeQuaternionEKF::ekf_h(const iDynTree::VectorDynSize& xhat_
     {
         double q0q3{q(0)*q(3)};
         double q1q2{q(1)*q(2)};
-        zhat_k_plus_one(3) = atan2(2*(q0q3+q1q2), 1 - 2*(q2squared + q3squared));
+        zhat_k_plus_one(3) = std::atan2(2*(q0q3+q1q2), 1 - 2*(q2squared + q3squared));
     }
 
     return true;
diff --git a/src/estimation/src/ExtendedKalmanFilter.cpp b/src/estimation/src/ExtendedKalmanFilter.cpp
index 854994343d2..32642c7b48a 100644
--- a/src/estimation/src/ExtendedKalmanFilter.cpp
+++ b/src/estimation/src/ExtendedKalmanFilter.cpp
@@ -144,8 +144,8 @@ bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfUpdate()
     auto xhat(toEigen(m_xhat));
     auto y(toEigen(m_y) - toEigen(z));        ///< innovation \f$ \tilde{y}_{k+1} = y_{k+1} - z_{k+1} \f$
 
-    S = H*Phat*H.transpose() + R;             ///< \f$ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R \f$
-    K = Phat*H.transpose()*S.inverse();       ///< \f$ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} \f$
+    S = H*Phat*(H.transpose()) + R;             ///< \f$ S_{k+1} = H_{k+1} \hat{P}_{k+1} H_{k+1}^T + R \f$
+    K = Phat*(H.transpose())*(S.inverse());       ///< \f$ K_{k+1} = \hat{P}_{k+1} H_{k+1}^T S_{k+1}^{-1} \f$
     P = Phat - (K*H*Phat);                    ///< \f$ P_{k+1} = \hat{P}_{k+1} - (K_{k+1} H \hat{P}_{k+1}) \f$
     x = xhat + K*y;                           ///< \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1} \tilde{y}_{k+1} \f$
 
diff --git a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
index e4473bde9bb..ef743430035 100644
--- a/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
+++ b/src/estimation/tests/AttitudeEstimatorUnitTest.cpp
@@ -13,6 +13,7 @@
 #include <iDynTree/Core/TestUtils.h>
 #include <iostream>
 #include <memory>
+
 void run(iDynTree::IAttitudeEstimator* estimator,
          const iDynTree::LinAcceleration& acc,
          const iDynTree::GyroscopeMeasurements& gyro,
@@ -81,7 +82,7 @@ int main()
     iDynTree::MagnetometerMeasurements mag; mag.zero();
     ok = qEKF->updateFilterWithMeasurements(linAcc, gyro, mag);
     ASSERT_IS_FALSE(ok);
-    linAcc(2) = 1.0;
+    linAcc(2) = -9.8;
     mag(2) = 1.0;
     std::cout << "Update measurements will internally run EKF update step" << std::endl;
     ok = qEKF->updateFilterWithMeasurements(linAcc, gyro, mag);
@@ -107,14 +108,35 @@ int main()
     }
 
     linAcc.zero();
-    linAcc(1) = 1;
-    gyro(1) = 0.05;
+    linAcc(2) = -9.8;
+    gyro(1) = 0.0;
 
     for (int i = 0; i <10 ; i++ )
     {
         run(qekf_, linAcc, gyro, mag);
     }
 
+    std::cout << "\nQuaternion EKF runs without faults." << std::endl;
+
+    std::cout << "\n\n Mahony filter running..." << std::endl;
+
+    std::unique_ptr<iDynTree::AttitudeMahonyFilter> mahony_filt;
+
+    mahony_filt = std::make_unique<iDynTree::AttitudeMahonyFilter>();
+    iDynTree::AttitudeMahonyFilterParameters mahony_params;
+    mahony_params.kp = 0.7;
+    mahony_params.ki = 0.01;
+    mahony_params.time_step_in_seconds = 0.01;
+    mahony_params.use_magnetometer_measurements = false;
+
+    mahony_filt->setParameters(mahony_params);
+    mahony_filt->setInternalState(x1_span);
+
+    iDynTree::IAttitudeEstimator* mahony_(mahony_filt.get());
+    for (int i = 0; i <10 ; i++ )
+    {
+        run(mahony_, linAcc, gyro, mag);
+    }
 
     return EXIT_SUCCESS;
 }

From 2cbb75f67a8a9b93428d93d941c8d1b65ba6a415 Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Mon, 5 Aug 2019 14:13:47 +0200
Subject: [PATCH 128/194] Feature: yarprobotstatepublisher reduced model tf
 optional parameter (#548)

* Add reduced-model optional parameter to consider only the link TFs

* Update reduced-model option handling

* Update src/tools/yarprobotstatepublisher/src/main.cpp

Update --reduced-model option text in help message

Co-Authored-By: Silvio Traversaro <pegua1@gmail.com>

* Update v0_12 release doc with reduced-model parameter
---
 doc/releases/v0_12.md                          |  1 +
 .../include/robotstatepublisher.h              |  3 +++
 src/tools/yarprobotstatepublisher/src/main.cpp | 12 +++++++-----
 .../src/robotstatepublisher.cpp                | 18 +++++++++++++++++-
 4 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 3370a7d4212..5a0c7d1374c 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -58,3 +58,4 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
 * `robot` option is deprecated, and replaced by `name-prefix`.
+* Add `reduced-model` optional parameter to stream only the link transformations to transform server. By default, tranformations from all the frames are streamed to the transform server.
diff --git a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
index 784d6c58fad..d6855ef36fc 100644
--- a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
+++ b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
@@ -54,6 +54,9 @@ class YARPRobotStatePublisherModule : public yarp::os::RFModule
     bool m_usingNetworkClock;
     yarp::os::NetworkClock m_netClock;
 
+    // Reduced flag option
+    bool reducedModelOption;
+
     // Class for computing forward kinematics
    iDynTree::KinDynComputations m_kinDynComp;
    iDynTree::VectorDynSize m_jointPos;
diff --git a/src/tools/yarprobotstatepublisher/src/main.cpp b/src/tools/yarprobotstatepublisher/src/main.cpp
index a310e33dcc1..2b816f2a454 100644
--- a/src/tools/yarprobotstatepublisher/src/main.cpp
+++ b/src/tools/yarprobotstatepublisher/src/main.cpp
@@ -35,11 +35,13 @@ int main(int argc, char *argv[])
     if (rf.check("help"))
     {
         cout<<"Options"<<endl;
-        cout<<"\t--name-prefix            <name-prefix>: prefix of the yarprobotstatepublisher ports (default no prefix)"<<endl;
-        cout<<"\t--tf-prefix              <tf-prefix>: prefix of the published TFs (default no prefix)"<<endl;
-        cout<<"\t--model                  <file-name>: file name of the model to load at startup"<<endl;
-        cout<<"\t--base-frame             <frame-name>: specify the base frame of the published tf tree"<<endl;
-        cout<<"\t--jointstates-topic      <topic-name>: source ROS topic that streams the joint state (default /joint_states)"<<endl;
+        cout<<"\t--name-prefix            <name-prefix> : prefix of the yarprobotstatepublisher ports (default no prefix)"<<endl;
+        cout<<"\t--tf-prefix              <tf-prefix>   : prefix of the published TFs (default no prefix)"<<endl;
+        cout<<"\t--model                  <file-name>   : file name of the model to load at startup"<<endl;
+        cout<<"\t--reduced-model                        : use the option to stream only the link TFs\n"
+              "                                           \t By default TFs of all the frames in the model are streamed"<<endl;
+        cout<<"\t--base-frame             <frame-name>  : specify the base frame of the published tf tree"<<endl;
+        cout<<"\t--jointstates-topic      <topic-name>  : source ROS topic that streams the joint state (default /joint_states)"<<endl;
         return EXIT_SUCCESS;
     }
 
diff --git a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
index ff32bbb1513..696a16a4aca 100644
--- a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
+++ b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
@@ -150,6 +150,11 @@ bool YARPRobotStatePublisherModule::configure(ResourceFinder &rf)
         return false;
     }
 
+    // Set reduced model option
+    // By default TFs of all the frames in the model are streamed
+    // If the option is present, only the TFs of the links are streamed to transform server
+    this->reducedModelOption=rf.check("reduced-model");
+
     // Setup the topic and configureisValid the onRead callback
     string jointStatesTopicName = rf.check("jointstates-topic",Value("/joint_states")).asString();
     m_jointStateSubscriber.reset(new JointStateSubscriber());
@@ -239,7 +244,18 @@ void YARPRobotStatePublisherModule::onRead(yarp::rosmsg::sensor_msgs::JointState
     // Set the updated joint positions
     m_kinDynComp.setJointPos(m_jointPos);
 
-    for (size_t frameIdx=0; frameIdx < model.getNrOfFrames(); frameIdx++)
+    // Set the size of the tf frames to be published
+    size_t sizeOfTFFrames;
+    if (this->reducedModelOption)
+    {
+        sizeOfTFFrames = model.getNrOfLinks();
+    }
+    else
+    {
+        sizeOfTFFrames = model.getNrOfFrames();
+    }
+
+    for (size_t frameIdx=0; frameIdx < sizeOfTFFrames; frameIdx++)
     {
         if(m_baseFrameIndex == frameIdx)    // skip self-tranform
             continue;

From 219916a3a37f00e7d05eb7cdae00aa7838eec820 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 13 Aug 2019 18:36:27 +0200
Subject: [PATCH 129/194] Add external vendored dependency on the fpconv
 library

This dependency is necessary to convert a double to its shortest string exact
representation. It can be probably removed as soon as std::to_chars is widely
available.
---
 CMakeLists.txt         |   2 +-
 extern/CMakeLists.txt  |   8 +
 extern/fpconv/fpconv.c | 355 +++++++++++++++++++++++++++++++++++++++++
 extern/fpconv/fpconv.h |  64 ++++++++
 extern/fpconv/powers.h | 111 +++++++++++++
 5 files changed, 539 insertions(+), 1 deletion(-)
 create mode 100644 extern/fpconv/fpconv.c
 create mode 100644 extern/fpconv/fpconv.h
 create mode 100644 extern/fpconv/powers.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6ea5b6b79c3..988a7d6514e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,7 +8,7 @@
 
 cmake_minimum_required(VERSION 3.5)
 
-project(iDynTree CXX)
+project(iDynTree C CXX)
 
 # Disable in source build, unless Eclipse is used
 if( ("${PROJECT_SOURCE_DIR}" STREQUAL "${PROJECT_BINARY_DIR}") AND
diff --git a/extern/CMakeLists.txt b/extern/CMakeLists.txt
index 4d644676691..2bbd889a30b 100644
--- a/extern/CMakeLists.txt
+++ b/extern/CMakeLists.txt
@@ -13,3 +13,11 @@ if( (IDYNTREE_USES_OCTAVE OR IDYNTREE_USES_MATLAB OR IDYNTREE_GENERATE_MATLAB) A
     # We save the location of MOxUnit in a variable that is then accessible to the test
     set(IDYNTREE_INTERNAL_MOXUNIT_PATH "${CMAKE_CURRENT_LIST_DIR}/MOxUnit/MOxUnit" PARENT_SCOPE)
 endif()
+
+# fpconv ( https://github.com/night-shift/fpconv ) is a small implementation of a shortest exact representation of double to string
+# as of C++14, it has no equivalent in the C++ standaard, as using stringstream with setprecision(17) result
+# in printing long numbers (such as 1.000000000000000000000) even when there is a short equivalent representation (1.0)
+# This can be removed once std::to_chars from the charconv header introduced in C++17 becomes widely available
+# We imported the MIT-licensed source code from https://github.com/night-shift/fpconv/tree/4a087d1b2df765baa409536931916a2c082cdda4
+add_library(idyntree-private-fpconv OBJECT fpconv/fpconv.c)
+target_include_directories(idyntree-private-fpconv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/fpconv)
diff --git a/extern/fpconv/fpconv.c b/extern/fpconv/fpconv.c
new file mode 100644
index 00000000000..53d87c6db61
--- /dev/null
+++ b/extern/fpconv/fpconv.c
@@ -0,0 +1,355 @@
+// The MIT License
+//
+// Copyright (c) 2013 Andreas Samoljuk
+//
+// Permission is hereby granted, free of charge, to any person obtaining
+// a copy of this software and associated documentation files (the
+// "Software"), to deal in the Software without restriction, including
+// without limitation the rights to use, copy, modify, merge, publish,
+// distribute, sublicense, and/or sell copies of the Software, and to
+// permit persons to whom the Software is furnished to do so, subject to
+// the following conditions:
+//
+// The above copyright notice and this permission notice shall be
+// included in all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+#include <stdbool.h>
+#include <string.h>
+
+#include "fpconv.h"
+#include "powers.h"
+
+#define fracmask  0x000FFFFFFFFFFFFFU
+#define expmask   0x7FF0000000000000U
+#define hiddenbit 0x0010000000000000U
+#define signmask  0x8000000000000000U
+#define expbias   (1023 + 52)
+
+#define absv(n) ((n) < 0 ? -(n) : (n))
+#define minv(a, b) ((a) < (b) ? (a) : (b))
+
+static uint64_t tens[] = {
+    10000000000000000000U, 1000000000000000000U, 100000000000000000U,
+    10000000000000000U, 1000000000000000U, 100000000000000U,
+    10000000000000U, 1000000000000U, 100000000000U,
+    10000000000U, 1000000000U, 100000000U,
+    10000000U, 1000000U, 100000U,
+    10000U, 1000U, 100U,
+    10U, 1U
+};
+
+static inline uint64_t get_dbits(double d)
+{
+    union {
+        double   dbl;
+        uint64_t i;
+    } dbl_bits = { d };
+
+    return dbl_bits.i;
+}
+
+static Fp build_fp(double d)
+{
+    uint64_t bits = get_dbits(d);
+
+    Fp fp;
+    fp.frac = bits & fracmask;
+    fp.exp = (bits & expmask) >> 52;
+
+    if(fp.exp) {
+        fp.frac += hiddenbit;
+        fp.exp -= expbias;
+
+    } else {
+        fp.exp = -expbias + 1;
+    }
+
+    return fp;
+}
+
+static void normalize(Fp* fp)
+{
+    while ((fp->frac & hiddenbit) == 0) {
+        fp->frac <<= 1;
+        fp->exp--;
+    }
+
+    int shift = 64 - 52 - 1;
+    fp->frac <<= shift;
+    fp->exp -= shift;
+}
+
+static void get_normalized_boundaries(Fp* fp, Fp* lower, Fp* upper)
+{
+    upper->frac = (fp->frac << 1) + 1;
+    upper->exp  = fp->exp - 1;
+
+    while ((upper->frac & (hiddenbit << 1)) == 0) {
+        upper->frac <<= 1;
+        upper->exp--;
+    }
+
+    int u_shift = 64 - 52 - 2;
+
+    upper->frac <<= u_shift;
+    upper->exp = upper->exp - u_shift;
+
+
+    int l_shift = fp->frac == hiddenbit ? 2 : 1;
+
+    lower->frac = (fp->frac << l_shift) - 1;
+    lower->exp = fp->exp - l_shift;
+
+
+    lower->frac <<= lower->exp - upper->exp;
+    lower->exp = upper->exp;
+}
+
+static Fp multiply(Fp* a, Fp* b)
+{
+    const uint64_t lomask = 0x00000000FFFFFFFF;
+
+    uint64_t ah_bl = (a->frac >> 32)    * (b->frac & lomask);
+    uint64_t al_bh = (a->frac & lomask) * (b->frac >> 32);
+    uint64_t al_bl = (a->frac & lomask) * (b->frac & lomask);
+    uint64_t ah_bh = (a->frac >> 32)    * (b->frac >> 32);
+
+    uint64_t tmp = (ah_bl & lomask) + (al_bh & lomask) + (al_bl >> 32); 
+    /* round up */
+    tmp += 1U << 31;
+
+    Fp fp = {
+        ah_bh + (ah_bl >> 32) + (al_bh >> 32) + (tmp >> 32),
+        a->exp + b->exp + 64
+    };
+
+    return fp;
+}
+
+static void round_digit(char* digits, int ndigits, uint64_t delta, uint64_t rem, uint64_t kappa, uint64_t frac)
+{
+    while (rem < frac && delta - rem >= kappa &&
+           (rem + kappa < frac || frac - rem > rem + kappa - frac)) {
+
+        digits[ndigits - 1]--;
+        rem += kappa;
+    }
+}
+
+static int generate_digits(Fp* fp, Fp* upper, Fp* lower, char* digits, int* K)
+{
+    uint64_t wfrac = upper->frac - fp->frac;
+    uint64_t delta = upper->frac - lower->frac;
+
+    Fp one;
+    one.frac = 1ULL << -upper->exp;
+    one.exp  = upper->exp;
+
+    uint64_t part1 = upper->frac >> -one.exp;
+    uint64_t part2 = upper->frac & (one.frac - 1);
+
+    int idx = 0, kappa = 10;
+    uint64_t* divp;
+    /* 1000000000 */
+    for(divp = tens + 10; kappa > 0; divp++) {
+
+        uint64_t div = *divp;
+        unsigned digit = part1 / div;
+
+        if (digit || idx) {
+            digits[idx++] = digit + '0';
+        }
+
+        part1 -= digit * div;
+        kappa--;
+
+        uint64_t tmp = (part1 <<-one.exp) + part2;
+        if (tmp <= delta) {
+            *K += kappa;
+            round_digit(digits, idx, delta, tmp, div << -one.exp, wfrac);
+
+            return idx;
+        }
+    }
+
+    /* 10 */
+    uint64_t* unit = tens + 18;
+
+    while(true) {
+        part2 *= 10;
+        delta *= 10;
+        kappa--;
+
+        unsigned digit = part2 >> -one.exp;
+        if (digit || idx) {
+            digits[idx++] = digit + '0';
+        }
+
+        part2 &= one.frac - 1;
+        if (part2 < delta) {
+            *K += kappa;
+            round_digit(digits, idx, delta, part2, one.frac, wfrac * *unit);
+
+            return idx;
+        }
+
+        unit--;
+    }
+}
+
+static int grisu2(double d, char* digits, int* K)
+{
+    Fp w = build_fp(d);
+
+    Fp lower, upper;
+    get_normalized_boundaries(&w, &lower, &upper);
+
+    normalize(&w);
+
+    int k;
+    Fp cp = find_cachedpow10(upper.exp, &k);
+
+    w     = multiply(&w,     &cp);
+    upper = multiply(&upper, &cp);
+    lower = multiply(&lower, &cp);
+
+    lower.frac++;
+    upper.frac--;
+
+    *K = -k;
+
+    return generate_digits(&w, &upper, &lower, digits, K);
+}
+
+static int emit_digits(char* digits, int ndigits, char* dest, int K, bool neg)
+{
+    int exp = absv(K + ndigits - 1);
+
+    /* write plain integer */
+    if(K >= 0 && (exp < (ndigits + 7))) {
+        memcpy(dest, digits, ndigits);
+        memset(dest + ndigits, '0', K);
+
+        return ndigits + K;
+    }
+
+    /* write decimal w/o scientific notation */
+    if(K < 0 && (K > -7 || exp < 4)) {
+        int offset = ndigits - absv(K);
+        /* fp < 1.0 -> write leading zero */
+        if(offset <= 0) {
+            offset = -offset;
+            dest[0] = '0';
+            dest[1] = '.';
+            memset(dest + 2, '0', offset);
+            memcpy(dest + offset + 2, digits, ndigits);
+
+            return ndigits + 2 + offset;
+
+        /* fp > 1.0 */
+        } else {
+            memcpy(dest, digits, offset);
+            dest[offset] = '.';
+            memcpy(dest + offset + 1, digits + offset, ndigits - offset);
+
+            return ndigits + 1;
+        }
+    }
+
+    /* write decimal w/ scientific notation */
+    ndigits = minv(ndigits, 18 - neg);
+
+    int idx = 0;
+    dest[idx++] = digits[0];
+
+    if(ndigits > 1) {
+        dest[idx++] = '.';
+        memcpy(dest + idx, digits + 1, ndigits - 1);
+        idx += ndigits - 1;
+    }
+
+    dest[idx++] = 'e';
+
+    char sign = K + ndigits - 1 < 0 ? '-' : '+';
+    dest[idx++] = sign;
+
+    int cent = 0;
+
+    if(exp > 99) {
+        cent = exp / 100;
+        dest[idx++] = cent + '0';
+        exp -= cent * 100;
+    }
+    if(exp > 9) {
+        int dec = exp / 10;
+        dest[idx++] = dec + '0';
+        exp -= dec * 10;
+
+    } else if(cent) {
+        dest[idx++] = '0';
+    }
+
+    dest[idx++] = exp % 10 + '0';
+
+    return idx;
+}
+
+static int filter_special(double fp, char* dest)
+{
+    if(fp == 0.0) {
+        dest[0] = '0';
+        return 1;
+    }
+
+    uint64_t bits = get_dbits(fp);
+
+    bool nan = (bits & expmask) == expmask;
+
+    if(!nan) {
+        return 0;
+    }
+
+    if(bits & fracmask) {
+        dest[0] = 'n'; dest[1] = 'a'; dest[2] = 'n';
+
+    } else {
+        dest[0] = 'i'; dest[1] = 'n'; dest[2] = 'f';
+    }
+
+    return 3;
+}
+
+int idyntree_private_fpconv_dtoa(double d, char dest[24])
+{
+    char digits[18];
+
+    int str_len = 0;
+    bool neg = false;
+
+    if(get_dbits(d) & signmask) {
+        dest[0] = '-';
+        str_len++;
+        neg = true;
+    }
+
+    int spec = filter_special(d, dest + str_len);
+
+    if(spec) {
+        return str_len + spec;
+    }
+
+    int K = 0;
+    int ndigits = grisu2(d, digits, &K);
+
+    str_len += emit_digits(digits, ndigits, dest + str_len, K, neg);
+
+    return str_len;
+}
diff --git a/extern/fpconv/fpconv.h b/extern/fpconv/fpconv.h
new file mode 100644
index 00000000000..b9e384ca055
--- /dev/null
+++ b/extern/fpconv/fpconv.h
@@ -0,0 +1,64 @@
+// The MIT License
+//
+// Copyright (c) 2013 Andreas Samoljuk
+//
+// Permission is hereby granted, free of charge, to any person obtaining
+// a copy of this software and associated documentation files (the
+// "Software"), to deal in the Software without restriction, including
+// without limitation the rights to use, copy, modify, merge, publish,
+// distribute, sublicense, and/or sell copies of the Software, and to
+// permit persons to whom the Software is furnished to do so, subject to
+// the following conditions:
+//
+// The above copyright notice and this permission notice shall be
+// included in all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+#ifndef FPCONV_H
+#define FPCONV_H
+
+/* Fast and accurate double to string conversion based on Florian Loitsch's
+ * Grisu-algorithm[1].
+ *
+ * Input:
+ * fp -> the double to convert, dest -> destination buffer.
+ * The generated string will never be longer than 24 characters.
+ * Make sure to pass a pointer to at least 24 bytes of memory.
+ * The emitted string will not be null terminated.
+ *
+ * Output:
+ * The number of written characters.
+ *
+ * Exemplary usage:
+ *
+ * void print(double d)
+ * {
+ *      char buf[24 + 1] // plus null terminator
+ *      int str_len = fpconv_dtoa(d, buf);
+ *
+ *      buf[str_len] = '\0';
+ *      printf("%s", buf);
+ * }
+ *
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int idyntree_private_fpconv_dtoa(double fp, char dest[24]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+/* [1] http://florian.loitsch.com/publications/dtoa-pldi2010.pdf */
diff --git a/extern/fpconv/powers.h b/extern/fpconv/powers.h
new file mode 100644
index 00000000000..eb758a57c63
--- /dev/null
+++ b/extern/fpconv/powers.h
@@ -0,0 +1,111 @@
+// The MIT License
+//
+// Copyright (c) 2013 Andreas Samoljuk
+//
+// Permission is hereby granted, free of charge, to any person obtaining
+// a copy of this software and associated documentation files (the
+// "Software"), to deal in the Software without restriction, including
+// without limitation the rights to use, copy, modify, merge, publish,
+// distribute, sublicense, and/or sell copies of the Software, and to
+// permit persons to whom the Software is furnished to do so, subject to
+// the following conditions:
+//
+// The above copyright notice and this permission notice shall be
+// included in all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+
+#include <stdint.h>
+
+#define npowers     87
+#define steppowers  8
+#define firstpower -348 /* 10 ^ -348 */
+
+#define expmax     -32
+#define expmin     -60
+
+
+typedef struct Fp {
+    uint64_t frac;
+    int exp;
+} Fp;
+
+static Fp powers_ten[] = {
+    { 18054884314459144840U, -1220 }, { 13451937075301367670U, -1193 },
+    { 10022474136428063862U, -1166 }, { 14934650266808366570U, -1140 },
+    { 11127181549972568877U, -1113 }, { 16580792590934885855U, -1087 },
+    { 12353653155963782858U, -1060 }, { 18408377700990114895U, -1034 },
+    { 13715310171984221708U, -1007 }, { 10218702384817765436U, -980 },
+    { 15227053142812498563U, -954 },  { 11345038669416679861U, -927 },
+    { 16905424996341287883U, -901 },  { 12595523146049147757U, -874 },
+    { 9384396036005875287U,  -847 },  { 13983839803942852151U, -821 },
+    { 10418772551374772303U, -794 },  { 15525180923007089351U, -768 },
+    { 11567161174868858868U, -741 },  { 17236413322193710309U, -715 },
+    { 12842128665889583758U, -688 },  { 9568131466127621947U,  -661 },
+    { 14257626930069360058U, -635 },  { 10622759856335341974U, -608 },
+    { 15829145694278690180U, -582 },  { 11793632577567316726U, -555 },
+    { 17573882009934360870U, -529 },  { 13093562431584567480U, -502 },
+    { 9755464219737475723U,  -475 },  { 14536774485912137811U, -449 },
+    { 10830740992659433045U, -422 },  { 16139061738043178685U, -396 },
+    { 12024538023802026127U, -369 },  { 17917957937422433684U, -343 },
+    { 13349918974505688015U, -316 },  { 9946464728195732843U,  -289 },
+    { 14821387422376473014U, -263 },  { 11042794154864902060U, -236 },
+    { 16455045573212060422U, -210 },  { 12259964326927110867U, -183 },
+    { 18268770466636286478U, -157 },  { 13611294676837538539U, -130 },
+    { 10141204801825835212U, -103 },  { 15111572745182864684U, -77 },
+    { 11258999068426240000U, -50 },   { 16777216000000000000U, -24 },
+    { 12500000000000000000U,   3 },   { 9313225746154785156U,   30 },
+    { 13877787807814456755U,  56 },   { 10339757656912845936U,  83 },
+    { 15407439555097886824U, 109 },   { 11479437019748901445U, 136 },
+    { 17105694144590052135U, 162 },   { 12744735289059618216U, 189 },
+    { 9495567745759798747U,  216 },   { 14149498560666738074U, 242 },
+    { 10542197943230523224U, 269 },   { 15709099088952724970U, 295 },
+    { 11704190886730495818U, 322 },   { 17440603504673385349U, 348 },
+    { 12994262207056124023U, 375 },   { 9681479787123295682U,  402 },
+    { 14426529090290212157U, 428 },   { 10748601772107342003U, 455 },
+    { 16016664761464807395U, 481 },   { 11933345169920330789U, 508 },
+    { 17782069995880619868U, 534 },   { 13248674568444952270U, 561 },
+    { 9871031767461413346U,  588 },   { 14708983551653345445U, 614 },
+    { 10959046745042015199U, 641 },   { 16330252207878254650U, 667 },
+    { 12166986024289022870U, 694 },   { 18130221999122236476U, 720 },
+    { 13508068024458167312U, 747 },   { 10064294952495520794U, 774 },
+    { 14996968138956309548U, 800 },   { 11173611982879273257U, 827 },
+    { 16649979327439178909U, 853 },   { 12405201291620119593U, 880 },
+    { 9242595204427927429U,  907 },   { 13772540099066387757U, 933 },
+    { 10261342003245940623U, 960 },   { 15290591125556738113U, 986 },
+    { 11392378155556871081U, 1013 },  { 16975966327722178521U, 1039 },
+    { 12648080533535911531U, 1066 }
+};
+
+static Fp find_cachedpow10(int exp, int* k)
+{
+    const double one_log_ten = 0.30102999566398114;
+
+    int approx = -(exp + npowers) * one_log_ten;
+    int idx = (approx - firstpower) / steppowers;
+
+    while(1) {
+        int current = exp + powers_ten[idx].exp + 64;
+
+        if(current < expmin) {
+            idx++;
+            continue;
+        }
+
+        if(current > expmax) {
+            idx--;
+            continue;
+        }
+
+        *k = (firstpower + idx * steppowers);
+
+        return powers_ten[idx];
+    }
+}

From 310b0826ecd0a253a1e6676b0b629d3a31bfec30 Mon Sep 17 00:00:00 2001
From: Prashanth <prashanthramadoss@gmail.com>
Date: Sat, 27 Jul 2019 18:53:37 +0200
Subject: [PATCH 130/194] [estimation][ExtendedKalmanFilter] add dynamic reset

---
 doc/releases/v0_12.md                         |  1 +
 .../Estimation/AttitudeQuaternionEKF.h        |  1 +
 .../Estimation/ExtendedKalmanFilter.h         | 48 ++++++++++++++++++-
 src/estimation/src/AttitudeQuaternionEKF.cpp  |  9 ++++
 src/estimation/src/ExtendedKalmanFilter.cpp   | 48 ++++++++++++++++++-
 5 files changed, 105 insertions(+), 2 deletions(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 3370a7d4212..dc626b7f16b 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -42,6 +42,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame
 * Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
+* Added dynamic reset functionality to `DiscreteExtendedKalmanFilterHelper` class (https://github.com/robotology/idyntree/pull/553)
 
 #### `bindings`
 * Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)
diff --git a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
index 15ef2a90a6c..24fc436de5f 100644
--- a/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
+++ b/src/estimation/include/iDynTree/Estimation/AttitudeQuaternionEKF.h
@@ -253,6 +253,7 @@ namespace iDynTree
          * @return bool true/false if successful or not
          */
         bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) override;
+        bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F) override;
 
         /**
          * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance
diff --git a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
index ffd418ed820..d0f93f1df64 100644
--- a/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
+++ b/src/estimation/include/iDynTree/Estimation/ExtendedKalmanFilter.h
@@ -90,15 +90,22 @@ namespace iDynTree
         virtual bool ekf_h(const iDynTree::VectorDynSize& xhat_k_plus_one,
                              iDynTree::VectorDynSize& zhat_k_plus_one) = 0;
 
+        /**
+         * @overload
+         */
+        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0;
+
         /**
          * @brief Describes the system Jacobian necessary for the propagation of predicted state covariance
          *        The analytical Jacobian describing the partial derivative of the system propagation with respect to the state
+         *        and the system propagation with respect to the input
          * @note the detail of this function needs to be implemented by the child class
          * @param[in] x system state
+         * @param[in] u system input
          * @param[out] F system Jacobian
          * @return bool true/false if successful or not
          */
-        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F) = 0;
+        virtual bool ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u,  iDynTree::MatrixDynSize& F) = 0;
 
         /**
          * @brief Describes the measurement Jacobian necessary for computing Kalman gain and updating the predicted state and its covariance
@@ -153,6 +160,19 @@ namespace iDynTree
          */
         bool ekfInit();
 
+        /**
+         * @brief Initializes and resizes the internal buffers of this filter
+         * @warning this is a very crucial method of this class. This method sets the input size through ekfSetInputSize(),
+         *          output size through ekfSetOutputSize() and state dimension through ekfSetStateSize() with the specified parameters,
+         *          such that the corresponding matrices and vectors will resize themselves to their corresponding dimensions.
+         *          Failing to do so might result in memory leaks and may cause the program to crash
+         * @param[in] state_size state size
+         * @param[in] input_size input size
+         * @param[in] output_size output size
+         * @return bool true/false if successful or not
+         */
+        bool ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size);
+
         /**
          * @brief Resets the filter flags
          * The filter flags check if the filter was properly initialized, if the initial state was set,
@@ -161,6 +181,25 @@ namespace iDynTree
          */
         void ekfReset();
 
+        /**
+         * @brief Resets the filter flags, initializes and resizes internal buffers of the filter, and
+         *             sets initial state, initial state covariance, and system noise and measurement noise covariance matrices
+         * @warning size of the span for P0 and Q must be of the size (state size*state size), where * is the regular multiplication operator
+         * @warning size of the span for R must be of the size (ouput size*output size),  where * is the regular multiplication operator
+         * @warning the matrices from the span are built in row-major ordering.
+         *
+         * @note this method is particularly useful while working with hybrid systems,
+         *             where the size of the system state or the measurements keep evolving with time
+         *
+         */
+        bool ekfReset(const size_t& state_size,
+                                 const size_t& input_size,
+                                 const size_t& output_size,
+                                 const iDynTree::Span<double>& x0,
+                                 const iDynTree::Span<double>& P0,
+                                 const iDynTree::Span<double>& Q,
+                                 const iDynTree::Span<double>& R);
+
         /**
          * @brief Set measurement vector at every time step
          * the measurement vector size and output size should match
@@ -254,6 +293,13 @@ namespace iDynTree
          */
         bool ekfGetStateCovariance(const iDynTree::Span<double> &P) const;
 
+   protected:
+        /**
+        * function template to ignore unused parameters
+        */
+        template <typename T>
+        void ignore(T &&) { }
+
     private:
         size_t m_dim_X;                                ///< state dimension
         size_t m_dim_Y;                                ///< output dimenstion
diff --git a/src/estimation/src/AttitudeQuaternionEKF.cpp b/src/estimation/src/AttitudeQuaternionEKF.cpp
index a70c97af6cd..bb8e203368c 100644
--- a/src/estimation/src/AttitudeQuaternionEKF.cpp
+++ b/src/estimation/src/AttitudeQuaternionEKF.cpp
@@ -287,6 +287,15 @@ bool iDynTree::AttitudeQuaternionEKF::updateFilterWithMeasurements(const iDynTre
     return ok;
 }
 
+bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::VectorDynSize& u, iDynTree::MatrixDynSize& F)
+{
+    ignore(u);
+    ignore(x);
+    ignore(F);
+    return false;
+}
+
+
 bool iDynTree::AttitudeQuaternionEKF::ekfComputeJacobianF(iDynTree::VectorDynSize& x, iDynTree::MatrixDynSize& F)
 {
     using iDynTree::toEigen;
diff --git a/src/estimation/src/ExtendedKalmanFilter.cpp b/src/estimation/src/ExtendedKalmanFilter.cpp
index 854994343d2..a01c8bebde1 100644
--- a/src/estimation/src/ExtendedKalmanFilter.cpp
+++ b/src/estimation/src/ExtendedKalmanFilter.cpp
@@ -24,6 +24,53 @@ void iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset()
     m_initial_state_covariance_set = false;
 }
 
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit(const size_t& state_size, const size_t& input_size, const size_t& output_size)
+{
+    m_dim_X = state_size;
+    m_dim_U = input_size;
+    m_dim_Y = output_size;
+
+    return ekfInit();
+}
+
+bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset(const size_t& state_size, const size_t& input_size, const size_t& output_size,
+                                                                                                        const iDynTree::Span<double>& x0, const iDynTree::Span<double>& P0,
+                                                                                                        const iDynTree::Span<double>& Q, const iDynTree::Span<double>& R)
+{
+    m_is_initialized = false;
+    m_measurement_updated = false;
+    m_input_updated = false;
+    m_initial_state_set = false;
+    m_initial_state_covariance_set = false;
+
+    if (!ekfInit(state_size, input_size, output_size))
+    {
+        return false;
+    }
+
+    if (!ekfSetInitialState(x0))
+    {
+        return false;
+    }
+
+    if (!ekfSetStateCovariance(P0))
+    {
+        return false;
+    }
+
+    if (!ekfSetSystemNoiseCovariance(Q))
+    {
+        return false;
+    }
+
+    if (!ekfSetMeasurementNoiseCovariance(R))
+    {
+        return false;
+    }
+
+    return true;
+}
+
 
 bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit()
 {
@@ -252,7 +299,6 @@ bool iDynTree::DiscreteExtendedKalmanFilterHelper::ekfGetStates(const iDynTree::
 
     for (size_t i = 0; i < m_dim_X; i++)
     {
-        double temp{m_x(i)};
         x(i) = m_x(i);
     }
     return true;

From f888b72ee493cd8748a9def5b8d0bb24c1945652 Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Tue, 20 Aug 2019 14:18:44 +0200
Subject: [PATCH 131/194] Feature: yarprobotstatepublisher update joints size
 check (#551)

* Remove joint size check

* Initialize model joint positions buffer to zero and update commandline documentation

* Update changelog
---
 doc/releases/v0_12.md                                 |  3 +++
 src/tools/yarprobotstatepublisher/src/main.cpp        |  5 ++++-
 .../src/robotstatepublisher.cpp                       | 11 +++--------
 3 files changed, 10 insertions(+), 9 deletions(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 5a0c7d1374c..a7f96fa4d04 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -59,3 +59,6 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
 * `robot` option is deprecated, and replaced by `name-prefix`.
 * Add `reduced-model` optional parameter to stream only the link transformations to transform server. By default, tranformations from all the frames are streamed to the transform server.
+* Remove joint size check between model joints and joints in ROS topic given through `jointstates-topic` parameter.
+* Initialize the model joint positions values to zero and update the joint positions values in run time if the values are
+available in ROS topic given through `jointstates-topic` parameter.
diff --git a/src/tools/yarprobotstatepublisher/src/main.cpp b/src/tools/yarprobotstatepublisher/src/main.cpp
index 2b816f2a454..20837a40f89 100644
--- a/src/tools/yarprobotstatepublisher/src/main.cpp
+++ b/src/tools/yarprobotstatepublisher/src/main.cpp
@@ -41,7 +41,10 @@ int main(int argc, char *argv[])
         cout<<"\t--reduced-model                        : use the option to stream only the link TFs\n"
               "                                           \t By default TFs of all the frames in the model are streamed"<<endl;
         cout<<"\t--base-frame             <frame-name>  : specify the base frame of the published tf tree"<<endl;
-        cout<<"\t--jointstates-topic      <topic-name>  : source ROS topic that streams the joint state (default /joint_states)"<<endl;
+        cout<<"\t--jointstates-topic      <topic-name>  : source ROS topic that streams the joint state (default /joint_states)\n"
+              "                                           \t The position values of the model joints are initilized to Zero\n"
+              "                                           \t In runtime, the joint values from the ROS topic are used to set\n"
+              "                                           \t the position values of some of the model joints."<<endl;
         return EXIT_SUCCESS;
     }
 
diff --git a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
index 696a16a4aca..9659d8f8ca0 100644
--- a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
+++ b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
@@ -128,6 +128,9 @@ bool YARPRobotStatePublisherModule::configure(ResourceFinder &rf)
     // Resize the joint pos buffer
     m_jointPos.resize(m_kinDynComp.model().getNrOfPosCoords());
 
+    // Initilize the joint pos buffer to Zero
+    m_jointPos.zero();
+
     // Get the base frame information
     if (rf.check("base-frame"))
     {
@@ -217,14 +220,6 @@ void YARPRobotStatePublisherModule::onRead(yarp::rosmsg::sensor_msgs::JointState
         return;
     }
 
-    // Check if joint states contain at least as many joints of the model
-    if (v.name.size() < m_jointPos.size())
-    {
-        yError() << "Size mismatch. Model has " << m_jointPos.size()
-                 << " joints, while the received JointState message has " << v.name.size() << " joints.";
-        return;
-    }
-
     // TODO: this part can be drastically speed up.
     //      Possible improvements:
     //        * Add a map string --> indeces

From 2a259581e469d89da6a8e7ba2f8b1af14b65b04a Mon Sep 17 00:00:00 2001
From: Prashanth <prashanthramadoss@gmail.com>
Date: Tue, 20 Aug 2019 14:58:36 +0200
Subject: [PATCH 132/194] [Estimation] Add DiscreteKalmanFilterHelper class

---
 doc/releases/v0_12.md                         |   1 +
 src/estimation/CMakeLists.txt                 |   6 +-
 .../iDynTree/Estimation/KalmanFilter.h        | 238 +++++++++++++
 src/estimation/src/KalmanFilter.cpp           | 324 ++++++++++++++++++
 src/estimation/tests/CMakeLists.txt           |   1 +
 src/estimation/tests/KalmanFilterUnitTest.cpp | 131 +++++++
 6 files changed, 699 insertions(+), 2 deletions(-)
 create mode 100644 src/estimation/include/iDynTree/Estimation/KalmanFilter.h
 create mode 100644 src/estimation/src/KalmanFilter.cpp
 create mode 100644 src/estimation/tests/KalmanFilterUnitTest.cpp

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 5a0c7d1374c..48b4d88dd04 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -42,6 +42,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame
 * Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
+* Added `DiscreteKalmanFilterHelper` class for an implementation of a discrete, linear time-invariant Kalman Filter  (https://github.com/robotology/idyntree/pull/559)
 
 #### `bindings`
 * Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)
diff --git a/src/estimation/CMakeLists.txt b/src/estimation/CMakeLists.txt
index 07d35a809b2..68e1600e837 100644
--- a/src/estimation/CMakeLists.txt
+++ b/src/estimation/CMakeLists.txt
@@ -20,7 +20,8 @@ set(IDYNTREE_ESTIMATION_HEADERS include/iDynTree/Estimation/BerdyHelper.h
                                 include/iDynTree/Estimation/ExtendedKalmanFilter.h
                                 include/iDynTree/Estimation/AttitudeEstimator.h
                                 include/iDynTree/Estimation/AttitudeMahonyFilter.h
-                                include/iDynTree/Estimation/AttitudeQuaternionEKF.h)
+                                include/iDynTree/Estimation/AttitudeQuaternionEKF.h
+                                include/iDynTree/Estimation/KalmanFilter.h                                )
 
 set(IDYNTREE_ESTIMATION_PRIVATE_INCLUDES include/iDynTree/Estimation/AttitudeEstimatorUtils.h)
 
@@ -37,7 +38,8 @@ set(IDYNTREE_ESTIMATION_SOURCES src/BerdyHelper.cpp
                                 src/AttitudeEstimator.cpp
                                 src/AttitudeEstimatorUtils.cpp
                                 src/AttitudeMahonyFilter.cpp
-                                src/AttitudeQuaternionEKF.cpp)
+                                src/AttitudeQuaternionEKF.cpp
+                                src/KalmanFilter.cpp)
 
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_ESTIMATION_SOURCES})
 SOURCE_GROUP("Header Files" FILES ${IDYNTREE_ESTIMATION_HEADERS})
diff --git a/src/estimation/include/iDynTree/Estimation/KalmanFilter.h b/src/estimation/include/iDynTree/Estimation/KalmanFilter.h
new file mode 100644
index 00000000000..ca4b4718927
--- /dev/null
+++ b/src/estimation/include/iDynTree/Estimation/KalmanFilter.h
@@ -0,0 +1,238 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef KALMAN_FILTER_H
+#define KALMAN_FILTER_H
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/MatrixDynSize.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/EigenHelpers.h>
+#include <vector>
+
+namespace iDynTree
+{
+    /**
+     * @class DiscreteKalmanFilterHelper Time Invariant Discrete Kalman Filter with additive Gaussian noise
+     *
+     * The Kalman filter can be constructed by giving the system design matrices A, B, C and D.
+     * where A is the state transition matrix, B is the control input matrix,
+     * C is the output matrix and D the feed through matrix.
+     * These matrices can be used to describe a stochastic model for a linear dynamical system.
+     *
+     * \f[ x_{k+1} = A x_{k} + B u_{k} + w_k \f]
+     * \f[ y_{k+1} = C x_{k+1} + D u_{k} + v_k \f]
+     *
+     * Once the filter is constructed, the system noise and measurement noise covariance matrices can be set.
+     * The filter can be run, after setting initial state and state covariance matrix.
+     * The filter init method is called to check if the filter is properly configured and is ready to use.
+     *
+     * @warning care must be taken to design these matrices with proper dimensions, so that the filter does not crash
+     *
+     * Once the filter is configured, the filter algorithm can be run in a loop by,
+     * - setting input vector
+     * - prediction step which propagates the state through the modeled state dynamics
+     * - setting the measurement vector
+     * - correct the predicted states with the incoming measurements
+     *
+     */
+    class DiscreteKalmanFilterHelper
+    {
+    public:
+        DiscreteKalmanFilterHelper();
+
+        /**
+         * @brief Describes the state propagation for a given dynamical system
+         *       and the measurement model given the available measurements.
+         *        If state of the system is denoted by \f$ x \f$, the control input by \f$ u \f$
+         *        and the measurements by \f$ y \f$, then
+         *        the system dynamics is given as \f$ x_{k+1} = A x_{k} + B u_{k} + w_k \f$
+         *        and the measurement model is given by \f$ y_{k+1} = C x_{k+1} + D u_{k} + v_k \f$
+         *
+         * @note all matrices are assumed to be time-invariant
+         * @param[in] A state transition matrix
+         * @param[in] B control input matrix mapping inputs to states
+         * @param[in] C output matrix mapping states to measurements
+         * @param[in] D feed through matrix mapping inputs to measurements
+         * @return bool true/false if filter was constructed successfully or not
+         */
+        bool constructKalmanFilter(const iDynTree::MatrixDynSize& A,
+                                const iDynTree::MatrixDynSize& B,
+                                const iDynTree::MatrixDynSize& C,
+                                const iDynTree::MatrixDynSize& D);
+        /**
+         * @overload
+         */
+        bool constructKalmanFilter(const iDynTree::MatrixDynSize& A,
+                                const iDynTree::MatrixDynSize& B,
+                                const iDynTree::MatrixDynSize& C);
+        /**
+         * @overload
+         */
+        bool constructKalmanFilter(const iDynTree::MatrixDynSize& A,
+                                const iDynTree::MatrixDynSize& C);
+
+        /**
+         * @brief Set initial state of the Kalman filter
+         * @warning this method must be called before calling kfInit()
+         * @param[in] x0 initial state of dimension \f$ dim_x \times 1 \f$
+         * @return bool true/false successful or not
+         */
+        bool kfSetInitialState(const iDynTree::VectorDynSize& x0);
+
+        /**
+         * @brief Sets the state covariance matrix
+         * @warning this method must be called before calling kfInit()
+         * @param[in] P state covariance matrix of dimensions \f$ dim_x \times dim_x \f$
+         * @return bool true/false successful or not
+         */
+        bool kfSetStateCovariance(const iDynTree::MatrixDynSize& P);
+
+        /**
+         * @brief Sets the system noise covariance matrix
+         * @warning this method must be called before calling kfInit()
+         * @param[in] Q system noise covariance matrix of dimensions \f$ dim_x \times dim_x \f$
+         * @return bool true/false successful or not
+         */
+        bool kfSetSystemNoiseCovariance(const iDynTree::MatrixDynSize& Q);
+
+        /**
+         * @brief Sets the measurement noise covariance matrix
+         * @warning this method must be called before calling kfInit()
+         * @param[in] R measurement covariance matrix of dimensions \f$ dim_y \times dim_y \f$
+         * @return bool true/false successful or not
+         */
+        bool kfSetMeasurementNoiseCovariance(const iDynTree::MatrixDynSize& R);
+
+        /**
+         * @brief This method checks if the filter is properly constructed and configured
+         * i.e. if initial states and covariance matrices are set.
+         * @warning this method must be called before calling kfPredict() or kfUpdate()
+         * @return bool true/false successful or not
+         */
+        bool kfInit();
+
+        /**
+         * @brief Set inputs for the Kalman filter
+         *
+         * @param[in] u input vector of dimension \f$ dim_u \times 1 \f$
+         * @return bool true/false successful or not
+         */
+        bool kfSetInputVector(const iDynTree::VectorDynSize& u);
+
+        /**
+         * @brief Runs one step of the Discrete Kalman Filter prediction equation
+         *        described by
+         *        \f$ \hat{x}_{k+1} = A x_{k} + B u_{k} \f$
+         *        \f$ \hat{P}_{k+1} = A_k P_k A_k^T + Q \f$
+         *
+         * @warning this function can be called only after setting up the filter properly through kfInit() step
+         * @note in case if B matrix is constructed, this function should be once called every step,
+         *       after setting up the input vector using kfSetInputVector() method.
+         *
+         * @return bool true/false if successful or not
+         */
+        bool kfPredict();
+
+        /**
+         * @brief Set measurements for the Kalman filter
+         *
+         * @param[in] y input vector of dimension \f$ dim_y \times 1 \f$
+         * @return bool true/false successful or not
+         */
+        bool kfSetMeasurementVector(const iDynTree::VectorDynSize& y);
+
+        /**
+         * @brief Runs one step of the Discrete Kalman Filter update equation
+         *        described by
+         *        \f$ \tilde{y}_{k+1} = C \hat{x}_{k+1} + D u_{k} \f$
+         *        \f$ x_{k+1} = \hat{x}_{k+1} + K_{k+1}(\tilde{y}_{k+1} - z_{k+1}) \f$
+         *        \f$ P_{k+1} = (I - K_{k+1} C) \hat{P}_{k+1} \f$
+         * where K is the Kalman gain.
+         * @warning this function can be called only after setting up the filter properly through kfInit() step
+         * @note this function should be once called every step,
+         *       after setting up the measurements vector using kfSetMeasurementVector() method.
+         *
+         * @return bool true/false if successful or not
+         */
+        bool kfUpdate();
+
+        /**
+         * @brief Get system state
+         *
+         * @param[out] x system state of dimension \f$ dim_x \times 1 \f$
+         * @return bool true/false successful or not
+         */
+        bool kfGetStates(iDynTree::VectorDynSize &x);
+
+        /**
+         * @brief Get system state covariance
+         *
+         * @param[out] P system state covariance matrix of dimension \f$ dim_x \times dim_x \f$
+         * @return bool true/false successful or not
+         */
+        bool kfGetStateCovariance(iDynTree::MatrixDynSize &P);
+
+        /**
+         * @brief Reset Kalman filter
+         * Resets the Kalman filter and initializes with the internally stored states and
+         * matrices initially set by the user.
+         */
+        bool kfReset();
+
+        /**
+         * @brief Reset Kalman filter with the given arguments
+         * @warning the system matrices A, B, C and D are unchanged with the reset filter
+         *
+         * @param[in] x0 initial state vector of dimensions \f$ dim_x \times 1 \f$
+         * @param[in] P state covariance matrix of dimensions \f$ dim_x \times dim_x \f$
+         * @param[in] Q system noise covariance matrix of dimensions \f$ dim_x \times dim_x \f$
+         * @param[in] R measurement noise covariance matrix of dimensions \f$ dim_y \times dim_y \f$
+         */
+        bool kfReset(const iDynTree::VectorDynSize& x0, const iDynTree::MatrixDynSize& P0,
+                     const iDynTree::MatrixDynSize& Q, const iDynTree::MatrixDynSize& R);
+
+    private:
+        size_t m_dim_X;                                ///< state dimension
+        size_t m_dim_Y;                                ///< output dimenstion
+        size_t m_dim_U;                                ///< input dimension
+
+        iDynTree::VectorDynSize m_x;                   ///< state at time instant k
+        iDynTree::VectorDynSize m_x0;                  ///< buffer to store initial state
+        iDynTree::VectorDynSize m_u;                   ///< input at time instant k
+        iDynTree::VectorDynSize m_y;                   ///< measurements at time instant k
+
+        iDynTree::MatrixDynSize m_A;                   ///< state transition matrix
+        iDynTree::MatrixDynSize m_B;                   ///< control input matrix
+        iDynTree::MatrixDynSize m_C;                   ///< output matrix
+        iDynTree::MatrixDynSize m_D;                   ///< feedthrough matrix
+
+        iDynTree::MatrixDynSize m_P;                   ///< state covariance matrix
+        iDynTree::MatrixDynSize m_P0;                  ///< initial state covariance matrix
+        iDynTree::MatrixDynSize m_Q;                   ///< system noise covariance matrix
+        iDynTree::MatrixDynSize m_R;                   ///< measurement covariance matrix
+
+        bool m_is_initialized{false};                      ///< flag to check if filter is initialized
+
+        bool m_filter_constructed{false};                 ///< flag to check if the filter is constructed properly with the A, B, C, D matrices
+        bool m_initial_state_set{false};                        ///< flag to check if initial state is set
+        bool m_initial_state_covariance_set{false};               ///< flag to check if initial state covariance matrix is set
+        bool m_measurement_noise_covariance_matrix_set{false};  ///< flag to check if measurement noise covariance matrix is set
+        bool m_system_noise_covariance_matrix_set{false};       ///< flag to check if system noise covariance matrix is set
+
+        bool m_measurement_updated{false};                 ///< flag to check if measurement is updated
+        bool m_input_updated{false};                       ///< flag to check if input is updated
+
+        bool m_use_feed_through{false};                    ///< toggle to use feed through matrix D
+        bool m_use_control_input{false};                    ///< toggle to use control input matrix B
+    };
+}
+
+#endif
diff --git a/src/estimation/src/KalmanFilter.cpp b/src/estimation/src/KalmanFilter.cpp
new file mode 100644
index 00000000000..c1138ce31a5
--- /dev/null
+++ b/src/estimation/src/KalmanFilter.cpp
@@ -0,0 +1,324 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/KalmanFilter.h>
+
+iDynTree::DiscreteKalmanFilterHelper::DiscreteKalmanFilterHelper()
+{
+
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::constructKalmanFilter(const iDynTree::MatrixDynSize& A,
+                                                         const iDynTree::MatrixDynSize& B,
+                                                         const iDynTree::MatrixDynSize& C)
+{
+    m_use_feed_through = false;
+    iDynTree::MatrixDynSize D;
+    return constructKalmanFilter(A, B, C, D);
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::constructKalmanFilter(const iDynTree::MatrixDynSize& A, const iDynTree::MatrixDynSize& C)
+{
+    m_use_control_input = false;
+    m_use_feed_through = false;
+    iDynTree::MatrixDynSize B, D;
+    return constructKalmanFilter(A, B, C, D);
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::constructKalmanFilter(const iDynTree::MatrixDynSize& A, const iDynTree::MatrixDynSize& B, const iDynTree::MatrixDynSize& C, const iDynTree::MatrixDynSize& D)
+{
+    if (A.rows() != A.cols())
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "constructKalmanFilter", "Could not construct KF - ill-formed state transition matrix A, exiting.");
+        return false;
+    }
+    m_dim_X = A.rows();
+    m_A.resize(m_dim_X, m_dim_X);
+    m_A = A;
+    m_P.resize(m_dim_X, m_dim_X);
+    m_P.zero();
+    m_Q.resize(m_dim_X, m_dim_X);
+    m_Q.zero();
+
+    if (B.capacity() ==  0)
+    {
+        m_use_control_input = false;
+    }
+    else if (B.rows() != A.rows())
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "constructKalmanFilter", "Could not construct KF -  control input matrix B dimension mismatch, exiting.");
+        return false;
+    }
+    else
+    {
+        m_use_control_input = true;
+        m_B = B;
+    }
+
+    m_dim_U = m_B.cols();
+    m_u.resize(m_dim_U);
+    m_u.zero();
+
+    if (C.capacity() == 0 || (C.cols() != A.rows()))
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "constructKalmanFilter", "Could not construct KF -  output matrix C dimension mismatch, exiting.");
+        return false;
+    }
+    m_dim_Y = C.rows();
+    m_C = C;
+    m_y.resize(m_dim_Y);
+    m_y.zero();
+    m_R.resize(m_dim_Y, m_dim_Y);
+    m_R.zero();
+
+    if (D.capacity() ==  0)
+    {
+        m_use_feed_through = false;
+    }
+    else if ((D.rows() != C.rows()) || (D.cols() != B.rows()))
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "constructKalmanFilter", "Could not construct KF -  control feedthrough matrix D dimension mismatch, exiting.");
+        return false;
+    }
+    else
+    {
+        m_use_feed_through = true;
+        m_D = D;
+    }
+
+    m_filter_constructed = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfSetInitialState(const iDynTree::VectorDynSize& x0)
+{
+    if (x0.size() != m_dim_X)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetInitialState", "Could not set KF initial state - dimension mismatch");
+        return false;
+    }
+
+    m_x = m_x0 = x0;
+    m_initial_state_set = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfSetStateCovariance(const iDynTree::MatrixDynSize& P)
+{
+    if ( (P.rows() != m_dim_X) || (P.cols() != m_dim_X))
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetStateCovariance", "Could not set KF initial state covariance - dimension mismatch");
+        return false;
+    }
+
+    m_P = m_P0 = P;
+    m_initial_state_covariance_set = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfSetSystemNoiseCovariance(const iDynTree::MatrixDynSize& Q)
+{
+    if ( (Q.rows() != m_dim_X) || (Q.cols() != m_dim_X))
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetSystemNoiseCovariance", "Could not set KF system noise covariance - dimension mismatch");
+        return false;
+    }
+
+    m_Q = Q;
+    m_system_noise_covariance_matrix_set = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfSetMeasurementNoiseCovariance(const iDynTree::MatrixDynSize& R)
+{
+    if ( (R.rows() != m_dim_Y) || (R.cols() != m_dim_Y))
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetMeasurementNoiseCovariance", "Could not set KF measurement noise covariance - dimension mismatch");
+        return false;
+    }
+
+    m_R = R;
+    m_measurement_noise_covariance_matrix_set = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfInit()
+{
+    if (!m_filter_constructed)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfInit", "Please construct the filter first.");
+        return false;
+    }
+
+    if (!m_measurement_noise_covariance_matrix_set || !m_system_noise_covariance_matrix_set)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfInit", "Please set the noise covariance matrices Q and R.");
+        return false;
+    }
+
+    if (!m_initial_state_set || !m_initial_state_covariance_set)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfInit", "Please set the initial state and the state covariance matrix.");
+        return false;
+    }
+
+    m_is_initialized = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfSetInputVector(const iDynTree::VectorDynSize& u)
+{
+    if (!m_use_control_input)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetInput", "B matrix was not constructed properly");
+        return false;
+    }
+
+    if (u.size() != m_dim_U)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetInput", "Could not set inputs - dimensions mismatch");
+        return false;
+    }
+
+    m_u = u;
+    m_input_updated = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfPredict()
+{
+    if (!m_is_initialized)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfPredict", "filter not initialized.");
+        return false;
+    }
+
+    if (m_use_control_input)
+    {
+        if (!m_input_updated)
+        {
+            iDynTree::reportWarning("DiscreteKalmanFilterHelper", "kfPredict", "input not updated. using old input");
+        }
+    }
+
+    using iDynTree::toEigen;
+    auto x(toEigen(m_x));
+    auto P(toEigen(m_P));
+    auto A(toEigen(m_A));
+    auto Q(toEigen(m_Q));
+
+    if (m_use_control_input)
+    {
+        auto B(toEigen(m_B));
+        auto u(toEigen(m_u));
+        x = (A*x) + (B*u);
+    }
+    else
+    {
+        x = A*x;
+    }
+
+    // propagate covariance
+    P = A*P*(A.transpose()) + Q;
+    m_input_updated = false;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfSetMeasurementVector(const iDynTree::VectorDynSize& y)
+{
+    if (y.size() != m_dim_Y)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfSetInitialState", "Could not set measurement - dimension mismatch");
+        return false;
+    }
+
+    m_y = y;
+    m_measurement_updated = true;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfUpdate()
+{
+    if (!m_is_initialized)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfUpdate", "filter not initialized.");
+        return false;
+    }
+
+    if (!m_measurement_updated)
+    {
+        iDynTree::reportError("DiscreteKalmanFilterHelper", "kfUpdate", "measurements not updated.");
+        return false;
+    }
+
+    using iDynTree::toEigen;
+    iDynTree::VectorDynSize z_;
+    z_.resize(m_dim_Y);
+    auto z(toEigen(z_));
+    auto C(toEigen(m_C));
+    auto x(toEigen(m_x));
+    auto y(toEigen(m_y));
+    auto P(toEigen(m_P));
+    auto R(toEigen(m_R));
+
+    if (m_use_feed_through)
+    {
+        auto D(toEigen(m_D));
+        auto u(toEigen(m_u));
+        z = (C*x) + (D*u);
+    }
+    else
+    {
+        z = C*x;
+    }
+
+    auto innovation = y - z;
+    auto I =Eigen::MatrixXd::Identity(m_dim_X, m_dim_X);
+    auto S = C*P*(C.transpose()) + R;
+    auto K = P*(C.transpose())*(S.inverse());
+    x = x + (K*innovation);
+    P = (I - (K*C))*P;
+    m_measurement_updated = false;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfReset()
+{
+    return kfReset(m_x0, m_P0, m_Q, m_R);
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfReset(const iDynTree::VectorDynSize& x0,
+                                                   const iDynTree::MatrixDynSize& P0,
+                                                   const iDynTree::MatrixDynSize& Q,
+                                                   const iDynTree::MatrixDynSize& R)
+{
+    m_initial_state_set = m_initial_state_covariance_set = false;
+    m_measurement_noise_covariance_matrix_set = m_system_noise_covariance_matrix_set = false;
+    kfSetInitialState(x0);
+    kfSetStateCovariance(P0);
+    kfSetSystemNoiseCovariance(Q);
+    kfSetMeasurementNoiseCovariance(R);
+
+    return kfInit();
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfGetStateCovariance(iDynTree::MatrixDynSize& P)
+{
+    P.resize(m_P.rows(), m_P.cols());
+    P = m_P;
+    return true;
+}
+
+bool iDynTree::DiscreteKalmanFilterHelper::kfGetStates(iDynTree::VectorDynSize& x)
+{
+    x.resize(m_x.size());
+    x = m_x;
+    return true;
+}
diff --git a/src/estimation/tests/CMakeLists.txt b/src/estimation/tests/CMakeLists.txt
index ad1e7d8d718..ac883d6b56a 100644
--- a/src/estimation/tests/CMakeLists.txt
+++ b/src/estimation/tests/CMakeLists.txt
@@ -28,3 +28,4 @@ add_estimation_test(ExternalWrenchesEstimation)
 add_estimation_test(ExtWrenchesAndJointTorquesEstimator)
 add_estimation_test(SimpleLeggedOdometry)
 add_estimation_test(AttitudeEstimator)
+add_estimation_test(KalmanFilter)
diff --git a/src/estimation/tests/KalmanFilterUnitTest.cpp b/src/estimation/tests/KalmanFilterUnitTest.cpp
new file mode 100644
index 00000000000..e9f5962814e
--- /dev/null
+++ b/src/estimation/tests/KalmanFilterUnitTest.cpp
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Estimation/KalmanFilter.h>
+#include <iDynTree/Core/TestUtils.h>
+#include <iostream>
+#include <memory>
+
+#include <random>
+
+int main()
+{
+    iDynTree::DiscreteKalmanFilterHelper kf;
+    // state space is position and velocity of truck with constant input acceleration u = a
+    // x {k+1} = A x {k} + B u {k}
+    // x1 {k+1} = x1 {k} + dt x2 {k} + (0.5) (dt^2) u
+    // x2 {k+1} = x2 {k} + dt u
+
+    double dt{1.0}; ///< discretization_time_step_in_s
+    iDynTree::MatrixDynSize A;
+    A.resize(2, 2);
+    A(0, 0) = 1; A(0, 1) = dt;
+    A(1, 0) = 0; A(1, 1) = 1;
+
+    iDynTree::MatrixDynSize B;
+    B.resize(2, 1);
+    B(0, 0) = 0.5*dt*dt;
+    B(1, 0) = dt;
+
+    // noisy measurement of the truck is made at each step without any feedthrough
+    iDynTree::MatrixDynSize C, D;
+    C.resize(1, 2);
+    C(0, 0) = 1; C(0, 1) = 2;
+
+    bool ok{false};
+    ok = kf.constructKalmanFilter(A, B, C);
+    ASSERT_IS_TRUE(ok);
+    std::cout << "Kalman filter constructed successfully." << std::endl;
+
+    iDynTree::MatrixDynSize Q;
+    Q.resize(2, 2);
+    double system_noise_var{1.0};
+    iDynTree::toEigen(Q) = system_noise_var*Eigen::MatrixXd::Identity(2, 2);
+    ok = kf.kfSetSystemNoiseCovariance(Q);
+    ASSERT_IS_TRUE(ok);
+    std::cout << "System noise covariance matrix set." << std::endl;
+
+    iDynTree::MatrixDynSize R;
+    R.resize(1, 1);
+    double measurement_noise_var{1.0};
+    R(0, 0) = measurement_noise_var;
+    ok = kf.kfSetMeasurementNoiseCovariance(R);
+    ASSERT_IS_TRUE(ok);
+    std::cout << "Measurement noise covariance matrix set." << std::endl;
+
+    iDynTree::VectorDynSize x0;
+    x0.resize(2);
+    x0.zero();
+    ok = kf.kfSetInitialState(x0);
+    ASSERT_IS_TRUE(ok);
+    std::cout << "initial state set." << std::endl;
+
+    iDynTree::MatrixDynSize P0;
+    P0.resize(2, 2);
+    double state_var{1.0};
+    iDynTree::toEigen(P0) = state_var*Eigen::MatrixXd::Identity(2, 2);
+    ok = kf.kfSetStateCovariance(P0);
+    std::cout << "initial state covariance set." << std::endl;
+
+    ok = kf.kfInit();
+    ASSERT_IS_TRUE(ok);
+    std::cout << "Kalman filter initialized." << std::endl;
+
+    // adding Gaussian noise to measurement
+    // setting up constant seed for RNG
+    // snippet taken from https://www.musicdsp.org/en/latest/Synthesis/168-c-gaussian-noise-generation.html
+    std::srand(0.0);
+    /* Setup constants */
+    const static int q = 15;
+    const static double c1 = (1 << q) - 1;
+    const static double c2 = ((int)(c1 / 3)) + 1;
+    const static double c3 = 1.f / c1;
+
+    for (int i = 0; i <20 ; i++ )
+    {
+        std::cout << "\n\n\n Iteration: " << i << std::endl;
+        iDynTree::VectorDynSize u;
+        u.resize(1);
+        u.zero();
+        ok = kf.kfSetInputVector(u);
+        ASSERT_IS_TRUE(ok);
+        std::cout << "setting input." << std::endl;
+        ok = kf.kfPredict();
+        ASSERT_IS_TRUE(ok);
+        std::cout << "kf prediction step ... passed." << std::endl;
+
+        iDynTree::VectorDynSize x;
+        kf.kfGetStates(x);
+
+        iDynTree::VectorDynSize y;
+        y.resize(1);
+        y(0) = x(0);
+
+        // adding gaussian noise
+        // snippet taken from https://www.musicdsp.org/en/latest/Synthesis/168-c-gaussian-noise-generation.html
+        double random = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX) + 1);
+        double noise = (2.f * ((random * c2) + (random * c2) + (random * c2)) - 3.f * (c2 - 1.f)) * c3;
+        y(0) += noise;
+
+        ok = kf.kfSetMeasurementVector(y);
+        ASSERT_IS_TRUE(ok);
+        std::cout << "setting noisy output." << std::endl;
+        ok = kf.kfUpdate();
+        ASSERT_IS_TRUE(ok);
+        std::cout << "kf update step passed." << std::endl;
+
+        iDynTree::VectorDynSize xhat;
+        kf.kfGetStates(xhat);
+        std::cout << "Estimated position: " << xhat(0) << " Estimated velocity: " << xhat(1) << std::endl;
+    }
+
+    return EXIT_SUCCESS;
+}
+

From a25f21ee2c850b39c5a5c7505c6f51b8c96dfe80 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <pegua1@gmail.com>
Date: Tue, 10 Oct 2017 17:05:23 +0200
Subject: [PATCH 133/194] Add ModelExporter class, initial support for URDF
 export and tests

fixup
---
 doc/releases/v0_12.md                         |   3 +
 src/model_io/urdf/CMakeLists.txt              |  18 +-
 .../include/iDynTree/ModelIO/ModelExporter.h  | 159 ++++++++
 .../urdf/include/private/URDFModelExport.h    |  58 +++
 .../urdf/include/private/URDFParsingUtils.h   |  39 ++
 src/model_io/urdf/src/ModelExporter.cpp       | 119 ++++++
 src/model_io/urdf/src/URDFModelExport.cpp     | 345 ++++++++++++++++++
 src/model_io/urdf/tests/CMakeLists.txt        |   1 +
 .../urdf/tests/ModelExporterUnitTest.cpp      |  95 +++++
 9 files changed, 831 insertions(+), 6 deletions(-)
 create mode 100644 src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
 create mode 100644 src/model_io/urdf/include/private/URDFModelExport.h
 create mode 100644 src/model_io/urdf/src/ModelExporter.cpp
 create mode 100644 src/model_io/urdf/src/URDFModelExport.cpp
 create mode 100644 src/model_io/urdf/tests/ModelExporterUnitTest.cpp

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 5a0c7d1374c..1fdc55a5705 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -55,6 +55,9 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 ### `Model`
 * Implement `getTotalMass()` method for Model class
 
+### `modelio`
+* Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
+
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
 * `robot` option is deprecated, and replaced by `name-prefix`.
diff --git a/src/model_io/urdf/CMakeLists.txt b/src/model_io/urdf/CMakeLists.txt
index 90cab89a983..4bf11e1515c 100644
--- a/src/model_io/urdf/CMakeLists.txt
+++ b/src/model_io/urdf/CMakeLists.txt
@@ -14,6 +14,7 @@ endif()
 
 set(IDYNTREE_MODELIO_URDF_HEADERS include/iDynTree/ModelIO/URDFDofsImport.h
                                   include/iDynTree/ModelIO/ModelLoader.h
+                                  include/iDynTree/ModelIO/ModelExporter.h
                                   include/deprecated/iDynTree/ModelIO/URDFModelImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFGenericSensorsImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFSolidShapesImport.h)
@@ -29,7 +30,8 @@ set(IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS include/private/URDFDocument.h
                                           include/private/MaterialElement.h
                                           include/private/VisualElement.h
                                           include/private/GeometryElement.h
-                                          include/private/URDFParsingUtils.h)
+                                          include/private/URDFParsingUtils.h
+                                          include/private/URDFModelExport.h)
 
 set(IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES src/URDFDocument.cpp
                                               src/InertialElement.cpp
@@ -41,14 +43,15 @@ set(IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES src/URDFDocument.cpp
                                               src/ForceTorqueSensorElement.cpp
                                               src/MaterialElement.cpp
                                               src/VisualElement.cpp
-                                              src/GeometryElement.cpp
-                                              )
+                                              src/GeometryElement.cpp)
 
 set(IDYNTREE_MODELIO_URDF_SOURCES src/URDFDofsImport.cpp
                                   src/ModelLoader.cpp
+                                  src/ModelExporter.cpp
                                   src/deprecated/URDFModelImport.cpp
                                   src/deprecated/URDFGenericSensorsImport.cpp
-                                  src/deprecated/URDFSolidShapesImport.cpp)
+                                  src/deprecated/URDFSolidShapesImport.cpp
+                                  src/URDFModelExport.cpp)
 
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_MODELIO_URDF_SOURCES})
 SOURCE_GROUP("Source Files\\XML Elements" FILES ${IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES})
@@ -60,7 +63,7 @@ list(APPEND IDYNTREE_MODELIO_URDF_SOURCES ${IDYNTREE_MODELIO_URDF_XMLELEMENTS_SO
 # share headers with all iDynTree targets
 set(libraryname idyntree-modelio-urdf)
 
-add_library(${libraryname} ${IDYNTREE_MODELIO_URDF_SOURCES} ${IDYNTREE_MODELIO_URDF_HEADERS} ${IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS})
+add_library(${libraryname} ${IDYNTREE_MODELIO_URDF_SOURCES} ${IDYNTREE_MODELIO_URDF_HEADERS} ${IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS} $<TARGET_OBJECTS:idyntree-private-fpconv>)
 
 target_compile_features(${libraryname} PRIVATE cxx_auto_type cxx_delegating_constructors cxx_final cxx_lambdas cxx_lambda_init_captures)
 
@@ -72,11 +75,14 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
                                                   ${LIBXML2_INCLUDE_DIR})
 
 target_link_libraries(${libraryname} LINK_PUBLIC idyntree-core idyntree-model idyntree-sensors idyntree-modelio-xml
-                                     LINK_PRIVATE ${TinyXML_LIBRARIES})
+                                     LINK_PRIVATE ${LIBXML2_LIBRARIES})
 
 target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS} ${LIBXML2_DEFINITIONS})
 
 target_include_directories(${libraryname} PRIVATE ${EIGEN3_INCLUDE_DIR})
+# See https://stackoverflow.com/questions/38832528/transitive-target-include-directories-on-object-libraries
+# Can be removed with CMake 3.12
+target_include_directories(${libraryname} PRIVATE $<TARGET_PROPERTY:idyntree-private-fpconv,INTERFACE_INCLUDE_DIRECTORIES>)
 
 # Ensure that build include directories are always included before system ones
 get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
diff --git a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
new file mode 100644
index 00000000000..2a6b910515f
--- /dev/null
+++ b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
@@ -0,0 +1,159 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef IDYNTREE_MODEL_EXPORTER_H
+#define IDYNTREE_MODEL_EXPORTER_H
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/Sensors/Sensors.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Options for the iDynTree exporter.
+ */
+class ModelExporterOptions
+{
+public:
+    /**
+     * Specify the base link of the exported model.
+     *
+     * Differently from the iDynTree::Model class, some file formats (such as the URDF) represent the multibody structure
+     * using a directed tree representation, for which it is necessary to explicitly specify a base link.
+     *
+     * If this string is empty (default value), the default base link contained in the model will be used.
+     *
+     * Default value: "".
+     * Supported formats: urdf.
+     */
+    std::string baseLink;
+
+    /**
+     * Constructor.
+     */
+    ModelExporterOptions();
+
+};
+
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Helper class to export a model to the supported textual formats.
+ *
+ * Currently the only format supported for export is the URDF format,
+ * as it is described in http://wiki.ros.org/urdf/XML .
+ *
+ * Only iDynTree::Model classes that represent multibody system with no loops
+ * can be exported.
+ *
+ * Furthermore, currently the model exporter only exports a subset of the features
+ * supported in iDynTree::Model. In particular, the following features are not exported:
+ * * Additional frames
+ * * Visual and collision geometries
+ * * Sensors
+ * * Joint limits, damping and static friction.
+ *
+ *
+ */
+class ModelExporter
+{
+private:
+
+    class Pimpl;
+    std::unique_ptr<Pimpl> m_pimpl;
+
+public:
+
+    /**
+     * @name Constructor/Destructor
+     */
+    //@{
+
+    /**
+     * Constructor
+     */
+    ModelExporter();
+
+    ~ModelExporter();
+
+    //@}
+
+    /**
+     * @name Model exporting and definition methods
+     * This methods are used to export the structure of your model.
+     */
+    //@{
+    const ModelExporterOptions& exportingOptions() const;
+
+    void setExportingOptions(const ModelExporterOptions& options);
+
+    /**
+     * Specifies the model of the robot to export.
+     *
+     * @param[in] model The used model.
+     * @param[in] sensors The used sensors.
+     * @param[in] options The used options.
+     * @return true if all went well, false otherwise.
+     */
+    bool init(const Model& model,
+              const SensorsList& sensors=SensorsList(),
+              const ModelExporterOptions options=ModelExporterOptions());
+
+    /**
+     * Get the loaded model that will be exported.
+     *
+     */
+    const Model & model();
+
+    /**
+     * Get the loaded sensors that will be exported.
+     */
+    const SensorsList & sensors();
+
+    /**
+     * Return true if the model have been correctly loaded, and can be exported.
+     *
+     * @return True if the model was loaded correctly.
+     */
+    bool isValid();
+
+    /**
+     * Export the model of the robot to a string.
+     *
+     * @param modelString string containg the model of the robot.
+     * @param filetype type of the file to load, currently supporting only urdf type.
+     *
+     */
+    bool exportModelToString(std::string & modelString, const std::string filetype="urdf");
+
+    /**
+     * Export the model of the robot to an external file.
+     *
+     * @param filename path to the file to export.
+     *                 It can be either a relative filename with respect to the current working directory,
+     *                 or an absolute filename.
+     * @param filetype type of the file to load, currently supporting only urdf type.
+     *
+     */
+    bool exportModelToFile(const std::string & filename, const std::string filetype="urdf");
+    //@}
+};
+
+}
+
+#endif
diff --git a/src/model_io/urdf/include/private/URDFModelExport.h b/src/model_io/urdf/include/private/URDFModelExport.h
new file mode 100644
index 00000000000..8e3fc73a768
--- /dev/null
+++ b/src/model_io/urdf/include/private/URDFModelExport.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ */
+
+#ifndef IDYNTREE_URDF_MODEL_EXPORT_H
+#define IDYNTREE_URDF_MODEL_EXPORT_H
+
+#include <string>
+
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+namespace iDynTree
+
+{
+
+class Model;
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Export a iDynTree::Model object to a URDF file.
+ *
+ * @see iDynTree::ModelExporterOptions for more details on supported and default options.
+ *
+ * @warning This function does not support exporting sensor or solid shapes at the moment.
+ *
+ * @param[in] urdf_filename Path to the URDF file that will be created.
+ *                          It can be either a relative filename with respect to the current working directory,
+ *                          or an absolute filename.
+ * @param[in] options the iDynTree::ModelExporterOptions struct of options passed to the parser.
+ * @return true if all went ok, false otherwise.
+ */
+bool URDFFromModel(const iDynTree::Model & model,
+                   const std::string & urdf_filename,
+                   const ModelExporterOptions options=ModelExporterOptions());
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Export a iDynTree::Model object to a URDF string.
+ *
+ * @see iDynTree::ModelExporterOptions for more details on supported and default options.
+ *
+ * @warning This function does not support exporting sensor or solid shapes at the moment.
+ *
+ * @param[in] options the iDynTree::ModelExporterOptions struct of options passed to the parser.
+ * @return true if all went ok, false otherwise.
+ */
+bool URDFStringFromModel(const iDynTree::Model & output,
+                         std::string & urdf_string,
+                         const ModelExporterOptions options=ModelExporterOptions());
+
+
+}
+
+#endif
diff --git a/src/model_io/urdf/include/private/URDFParsingUtils.h b/src/model_io/urdf/include/private/URDFParsingUtils.h
index 67d4f9282f4..d3058e867c3 100644
--- a/src/model_io/urdf/include/private/URDFParsingUtils.h
+++ b/src/model_io/urdf/include/private/URDFParsingUtils.h
@@ -11,6 +11,7 @@
 #ifndef IDYNTREE_URDF_PARSING_UTILS_H
 #define IDYNTREE_URDF_PARSING_UTILS_H
 
+#include <cmath>
 #include <cstdlib>
 #include <sstream>
 #include <string>
@@ -19,6 +20,8 @@
 #include <iDynTree/Core/Transform.h>
 #include <iDynTree/Core/VectorFixSize.h>
 
+#include <fpconv.h>
+
 namespace iDynTree
 {
 
@@ -47,6 +50,21 @@ bool inline stringToUnsignedIntWithClassicLocale(const std::string & inStr, unsi
     return !(ss.fail());
 }
 
+bool inline doubleToStringWithClassicLocale(const double & inDouble, std::string& outStr)
+{
+    if (std::isnan(inDouble) || std::isinf(inDouble)) {
+        return false;
+    }
+
+    // fpconv returns nul-terminated strings, that can be converted directly to C++ std::string
+    // see https://github.com/night-shift/fpconv/tree/4a087d1b2df765baa409536931916a2c082cdda4#example-usage
+    char buf[24 + 1];
+    int str_len = idyntree_private_fpconv_dtoa(inDouble, buf);
+    buf[str_len] = '\0';
+    outStr = buf;
+    return true;
+}
+
 std::string inline intToString(const int inInt)
 {
     std::stringstream ss;
@@ -116,6 +134,27 @@ bool inline vector3FromString(const std::string & vector_str, Vector3 & out)
     return true;
 }
 
+template<typename iDynTreeVectorType>
+bool inline vectorToString(const iDynTreeVectorType & in, std::string & out_str)
+{
+    std::stringstream ss;
+    bool ok = true;
+    for (unsigned int i = 0; i < in.size(); ++i)
+    {
+        std::string bufStr;
+        ok = ok && doubleToStringWithClassicLocale(in(i), bufStr);
+        if (i != 0)
+        {
+            ss << " ";
+        }
+        ss << bufStr;
+    }
+
+    out_str = ss.str();
+
+    return ok;
+}
+
 
 bool inline vector4FromString(const std::string & vector_str, Vector4 & out)
 {
diff --git a/src/model_io/urdf/src/ModelExporter.cpp b/src/model_io/urdf/src/ModelExporter.cpp
new file mode 100644
index 00000000000..657a81a7d5e
--- /dev/null
+++ b/src/model_io/urdf/src/ModelExporter.cpp
@@ -0,0 +1,119 @@
+/*
+ * Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include "iDynTree/ModelIO/ModelExporter.h"
+
+#include "URDFModelExport.h"
+
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+
+ModelExporterOptions::ModelExporterOptions()
+: baseLink("") {}
+
+
+class ModelExporter::Pimpl {
+public:
+    Model m_model;
+    SensorsList m_sensors;
+    bool m_isModelValid;
+    ModelExporterOptions m_options;
+
+    bool setModelAndSensors(const Model& _model, const SensorsList& _sensors);
+};
+
+bool ModelExporter::Pimpl::setModelAndSensors(const Model& _model,
+                                                           const SensorsList& _sensors)
+{
+
+    m_model = _model;
+    m_sensors = _sensors;
+
+    m_isModelValid = true;
+
+    return true;
+}
+
+ModelExporter::ModelExporter()
+: m_pimpl(new Pimpl())
+{
+    m_pimpl->m_isModelValid = false;
+}
+
+ModelExporter::~ModelExporter() {}
+
+const Model& ModelExporter::model()
+{
+    return m_pimpl->m_model;
+}
+
+const SensorsList& ModelExporter::sensors()
+{
+    return m_pimpl->m_sensors;
+}
+
+bool ModelExporter::isValid()
+{
+    return m_pimpl->m_isModelValid;
+}
+
+const ModelExporterOptions& ModelExporter::exportingOptions() const
+{
+    return m_pimpl->m_options;
+}
+
+void ModelExporter::setExportingOptions(const ModelExporterOptions& options)
+{
+    m_pimpl->m_options = options;
+}
+
+bool ModelExporter::init(const Model& model,
+                         const SensorsList& sensors,
+                         const ModelExporterOptions options)
+{
+    m_pimpl->m_options = options;
+    return m_pimpl->setModelAndSensors(model, sensors);
+}
+
+bool ModelExporter::exportModelToString(std::string & modelString, const std::string filetype)
+{
+    if (filetype != "urdf") {
+        std::stringstream error_msg;
+        error_msg << "Filetype " << filetype << " not supported. Only urdf format is currently supported.";
+        reportError("ModelExporter", "exportModelToString", error_msg.str().c_str());
+        return false;
+    }
+
+    return URDFStringFromModel(m_pimpl->m_model, modelString, m_pimpl->m_options);
+}
+
+/**
+ * Export the model of the robot to an external file.
+ *
+ * @param filename path to the file to export
+ * @param filetype type of the file to load, currently supporting only urdf type.
+ *
+ */
+bool ModelExporter::exportModelToFile(const std::string & filename, const std::string filetype)
+{
+    if (filetype != "urdf") {
+        std::stringstream error_msg;
+        error_msg << "Filetype " << filetype << " not supported. Only urdf format is currently supported.";
+        reportError("ModelExporter", "exportModelToFile", error_msg.str().c_str());
+        return false;
+    }
+
+    return URDFFromModel(m_pimpl->m_model, filename, m_pimpl->m_options);
+}
+
+}
diff --git a/src/model_io/urdf/src/URDFModelExport.cpp b/src/model_io/urdf/src/URDFModelExport.cpp
new file mode 100644
index 00000000000..417ed718f6a
--- /dev/null
+++ b/src/model_io/urdf/src/URDFModelExport.cpp
@@ -0,0 +1,345 @@
+/*
+ * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ */
+
+#include "URDFModelExport.h"
+
+#include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Core/SpatialInertia.h>
+#include <iDynTree/Core/Transform.h>
+#include <iDynTree/Core/VectorFixSize.h>
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/Model/Traversal.h>
+#include <iDynTree/Model/FixedJoint.h>
+#include <iDynTree/Model/PrismaticJoint.h>
+#include <iDynTree/Model/RevoluteJoint.h>
+
+#include "URDFParsingUtils.h"
+
+
+#include <iomanip>
+#include <fstream>
+#include <string>
+
+#include <libxml/tree.h>
+#include <libxml/parser.h>
+
+namespace iDynTree
+{
+
+/*
+ * Add a <origin> URDF element to the specified parent element with the specified transform.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function
+ * modifies it to be something like:
+ * <parent_element>
+ *   <origin xyz="1.0 2.0 3.0" rpy="0.0 1.0 2.0" />
+ * </parent_element>
+ * where the actual values of the xyz and rpy attributes depend on the input trans argument.
+ */
+bool exportTransform(const Transform &trans, xmlNodePtr parent_element)
+{
+    bool ok = true;
+    xmlNodePtr origin = xmlNewChild(parent_element, NULL, BAD_CAST "origin", NULL);
+    std::string pose_xyz_str;
+    ok = ok && vectorToString(trans.getPosition(), pose_xyz_str);
+    std::string pose_rpy_str;
+    Vector3 pose_rpy = trans.getRotation().asRPY();
+    ok = ok && vectorToString(pose_rpy, pose_rpy_str);
+    xmlNewProp(origin, BAD_CAST "xyz", BAD_CAST pose_xyz_str.c_str());
+    xmlNewProp(origin, BAD_CAST "rpy", BAD_CAST pose_rpy_str.c_str());
+    return ok;
+}
+
+/*
+ * Add a <inertial> URDF element to the specified parent element with the specified 6D inertia.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
+ * <parent_element>
+ *   <inertial>
+ *     <origin xyz="0 0 0.5" rpy="0 0 0"/>
+ *     <mass value="1"/>
+ *     <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
+ *   </inertial>
+ * </parent_element>
+ * where the actual values of the origin, mass and inertia element's attributes depend on the input inertia argument.
+ */
+bool exportInertial(const SpatialInertia &inertia, xmlNodePtr parent_element)
+{
+    bool ok=true;
+    std::string bufStr;
+    xmlNodePtr inertial = xmlNewChild(parent_element, NULL, BAD_CAST "inertial", NULL);
+
+
+    xmlNodePtr mass_xml = xmlNewChild(inertial, NULL, BAD_CAST "mass", NULL);
+    ok = ok && doubleToStringWithClassicLocale(inertia.getMass(), bufStr);
+    xmlNewProp(mass_xml, BAD_CAST "value", BAD_CAST bufStr.c_str());
+
+    ok = ok && exportTransform(Transform(Rotation::Identity(), inertia.getCenterOfMass()), inertial);
+
+    xmlNodePtr inertia_xml = xmlNewChild(inertial, NULL, BAD_CAST "inertia", NULL);
+    RotationalInertiaRaw rotInertia = inertia.getRotationalInertiaWrtCenterOfMass();
+    ok = ok && doubleToStringWithClassicLocale(rotInertia(0, 0), bufStr);
+    xmlNewProp(inertia_xml, BAD_CAST "ixx", BAD_CAST bufStr.c_str());
+    ok = ok && doubleToStringWithClassicLocale(rotInertia(0, 1), bufStr);
+    xmlNewProp(inertia_xml, BAD_CAST "ixy", BAD_CAST bufStr.c_str());
+    ok = ok && doubleToStringWithClassicLocale(rotInertia(0, 2), bufStr);
+    xmlNewProp(inertia_xml, BAD_CAST "ixz", BAD_CAST bufStr.c_str());
+    ok = ok && doubleToStringWithClassicLocale(rotInertia(1, 1), bufStr);
+    xmlNewProp(inertia_xml, BAD_CAST "iyy", BAD_CAST bufStr.c_str());
+    ok = ok && doubleToStringWithClassicLocale(rotInertia(1, 2), bufStr);
+    xmlNewProp(inertia_xml, BAD_CAST "iyz", BAD_CAST bufStr.c_str());
+    ok = ok && doubleToStringWithClassicLocale(rotInertia(2, 2), bufStr);
+    xmlNewProp(inertia_xml, BAD_CAST "izz", BAD_CAST bufStr.c_str());
+
+    return ok;
+}
+
+/*
+ * Add a <link> URDF element to the specified parent element with the specified link info.
+ *
+ * Currently this function does not export visual and collision elements.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
+ *
+ * <parent_element>
+ *   <link name="my_link">
+ *     <inertial>
+ *       ...
+ *     </inertial>
+ *
+ *  </link>
+ * <parent_element/>
+ *
+ * where the actual values of the added element and attributes depend on the input link and linkName arguments.
+ */
+bool exportLink(const Link &link, const std::string linkName, xmlNodePtr parent_element)
+{
+    bool ok = true;
+    xmlNodePtr link_xml = xmlNewChild(parent_element, NULL, BAD_CAST "link", NULL);
+    xmlNewProp(link_xml, BAD_CAST "name", BAD_CAST linkName.c_str());
+
+    ok = ok && exportInertial(link.getInertia(), link_xml);
+
+    /* TODO(traversaro) : support solid shapes
+    for (std::size_t i = 0 ; i < link.visual_array.size() ; ++i)
+        exportVisual(*link.visual_array[i], link_xml);
+    for (std::size_t i = 0 ; i < link.collision_array.size() ; ++i)
+        exportCollision(*link.collision_array[i], link_xml);
+    */
+
+    return ok;
+}
+
+/**
+ * Add a <joint> URDF element to the specified parent element with the specified joint info.
+ *
+ * At the moment, only the URDF joint of types revolute, continuous, prismatic and fixed are supported, as they are the only one supported
+ * in the iDynTree::Model class. Furthermore, it is not supported the export of joint limits and friction.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
+ *
+ * <parent_element>
+ *   <joint name="my_joint" type="floating">
+ *      <origin xyz="0 0 1" rpy="0 0 3.1416"/>
+ *      <parent link="link1"/>
+ *      <child link="link2"/>
+ *      <calibration rising="0.0"/>
+ *      <dynamics damping="0.0" friction="0.0"/>
+ *      <limit effort="30" velocity="1.0" lower="-2.2" upper="0.7" />
+ *   </joint>
+ * <parent_element/>
+ *
+ * where the actual values of the added element and attributes depend on the input joint, parentLink and childLink arguments.
+ */
+bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr childLink, const Model& model, xmlNodePtr parent_element)
+{
+    bool ok=true;
+    xmlNodePtr joint_xml = xmlNewChild(parent_element, NULL, BAD_CAST "joint", NULL);
+    xmlNewProp(joint_xml, BAD_CAST "name", BAD_CAST model.getJointName(joint->getIndex()).c_str());
+
+    if (dynamic_cast<const FixedJoint*>(joint))
+    {
+        xmlNewProp(joint_xml, BAD_CAST "type", BAD_CAST "fixed");
+    }
+    else if (dynamic_cast<const RevoluteJoint*>(joint))
+    {
+        if(joint->hasPosLimits())
+        {
+            xmlNewProp(joint_xml, BAD_CAST "type", BAD_CAST "revolute");
+        }
+        else
+        {
+            xmlNewProp(joint_xml, BAD_CAST "type", BAD_CAST "continuous");
+        }
+    }
+    else if (dynamic_cast<const PrismaticJoint*>(joint))
+    {
+        xmlNewProp(joint_xml, BAD_CAST "type", BAD_CAST "prismatic");
+    }
+    else
+    {
+        std::cerr << "[ERROR] URDFModelExport: Impossible to convert joint of type "
+                  <<  typeid(joint).name() << " to a URDF joint " << std::endl;
+        return false;
+    }
+
+    // origin
+    exportTransform(joint->getRestTransform(parentLink->getIndex(), childLink->getIndex()), joint_xml);
+
+    if (joint->getNrOfDOFs() != 0)
+    {
+        Axis axis;
+
+        // Check that the axis of the joint is supported by URDF
+        if (dynamic_cast<const RevoluteJoint*>(joint))
+        {
+            const RevoluteJoint* revJoint = dynamic_cast<const RevoluteJoint*>(joint);
+            axis = revJoint->getAxis(childLink->getIndex());
+        }
+        else if (dynamic_cast<const PrismaticJoint*>(joint))
+        {
+            const PrismaticJoint* prismJoint = dynamic_cast<const PrismaticJoint*>(joint);
+            axis = prismJoint->getAxis(childLink->getIndex());
+        }
+        else
+        {
+            std::cerr << "[ERROR] URDFModelExport: Impossible to convert joint of type "
+                      <<  typeid(joint).name() << " to a URDF joint " << std::endl;
+            return false;
+        }
+
+        // Check that the axis is URDF-compatible
+        if (toEigen(axis.getOrigin()).norm() > 1e-7)
+        {
+            std::cerr << "[ERROR] URDFModelExport: Impossible to convert joint "
+                      <<   model.getJointName(joint->getIndex()) << " to a URDF joint without moving the link frame of link "
+                      << model.getLinkName(childLink->getIndex()) << " , because its origin is "
+                      << axis.getOrigin().toString()  << std::endl;
+            return false;
+        }
+
+        // axis
+        xmlNodePtr axis_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "axis", NULL);
+        std::string bufStr;
+        ok = ok && vectorToString(axis.getDirection(), bufStr);
+        xmlNewProp(axis_xml, BAD_CAST "xyz", BAD_CAST bufStr.c_str());
+    }
+
+    // parent
+    xmlNodePtr parent_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "parent", NULL);
+    xmlNewProp(parent_xml, BAD_CAST "link", BAD_CAST model.getLinkName(parentLink->getIndex()).c_str());
+
+    // child
+    xmlNodePtr child_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "child", NULL);
+    xmlNewProp(child_xml, BAD_CAST "link", BAD_CAST model.getLinkName(childLink->getIndex()).c_str());
+
+    // TODO(traversaro) : handle joint limits and friction
+
+    return ok;
+}
+
+bool URDFStringFromModel(const iDynTree::Model & model,
+                         std::string & urdf_string,
+                         const ModelExporterOptions options)
+{
+    bool ok = true;
+
+    xmlDocPtr urdf_xml = xmlNewDoc(BAD_CAST "1.0");
+    xmlNodePtr robot = xmlNewNode(NULL, BAD_CAST "robot");
+    // TODO(traversaro) properly export this :)
+    xmlNewProp(robot, BAD_CAST "name", BAD_CAST "iDynTreeURDFModelExportModelName");
+    xmlDocSetRootElement(urdf_xml, robot);
+
+    // TODO(traversaro) : We are assuming that the model has no loops,
+    //                    we should add a check for this
+
+    // URDF assumes a directed tree of the multibody model structure, so we need to assume that a given link is
+    // the base
+    std::string baseLink = options.baseLink;
+    if (baseLink.empty())
+    {
+        baseLink = model.getLinkName(model.getDefaultBaseLink());
+    }
+
+    LinkIndex baseLinkIndex = model.getLinkIndex(baseLink);
+    if (baseLinkIndex == LINK_INVALID_INDEX)
+    {
+        std::cerr << "[ERROR] URDFStringFromModel : specified baseLink " << baseLink << " is not part of the model" << std::endl;
+        xmlFreeDoc(urdf_xml);
+        return false;
+    }
+
+    // Create a Traversal
+    Traversal exportTraversal;
+    if (!model.computeFullTreeTraversal(exportTraversal, baseLinkIndex))
+    {
+        std::cerr << "[ERROR] URDFStringFromModel : error in computeFullTreeTraversal" << std::endl;
+        xmlFreeDoc(urdf_xml);
+        return false;
+    }
+
+
+    // Export links and joints following the traversal
+    for (TraversalIndex trvIdx=0; trvIdx < static_cast<TraversalIndex>(exportTraversal.getNrOfVisitedLinks()); trvIdx++)
+    {
+        LinkConstPtr visitedLink = exportTraversal.getLink(trvIdx);
+        std::string visitedLinkName = model.getLinkName(visitedLink->getIndex());
+
+        // Export parent joint, if this is not the base
+        if (trvIdx != 0)
+        {
+            LinkConstPtr parentLink = exportTraversal.getParentLink(trvIdx);
+            IJointConstPtr parentJoint = exportTraversal.getParentJoint(trvIdx);
+            ok = ok && exportJoint(parentJoint, parentLink, visitedLink,  model, robot);
+        }
+
+        // Export link
+        ok = ok && exportLink(*visitedLink, visitedLinkName, robot);
+    }
+
+    // TODO(traversaro) : export additional frames
+
+    xmlChar *xmlbuff=0;
+    int buffersize=0;
+    xmlDocDumpFormatMemory(urdf_xml, &xmlbuff, &buffersize, 1);
+    urdf_string.resize(buffersize);
+    std::memcpy((void*)urdf_string.data(), (void*)xmlbuff, buffersize);
+
+    xmlFree(xmlbuff);
+    xmlFreeDoc(urdf_xml);
+
+    return ok;
+}
+
+
+bool URDFFromModel(const iDynTree::Model & model,
+                   const std::string & urdf_filename,
+                   const ModelExporterOptions options)
+{
+    std::ofstream ofs(urdf_filename.c_str());
+
+    if( !ofs.is_open() )
+    {
+        std::cerr << "[ERROR] iDynTree::URDFFromModel : error opening file "
+                  << urdf_filename << std::endl;
+        return false;
+    }
+
+    std::string xml_string;
+    if (!URDFStringFromModel(model, xml_string, options))
+    {
+        return false;
+    }
+
+    // Write string to file
+    ofs << xml_string;
+    ofs.close();
+
+    return true;
+}
+
+
+}
diff --git a/src/model_io/urdf/tests/CMakeLists.txt b/src/model_io/urdf/tests/CMakeLists.txt
index fc1c0e40da3..939527fd55e 100644
--- a/src/model_io/urdf/tests/CMakeLists.txt
+++ b/src/model_io/urdf/tests/CMakeLists.txt
@@ -24,6 +24,7 @@ macro(add_modelio_urdf_unit_test classname)
 endmacro()
 
 add_modelio_urdf_unit_test(URDFModelImport)
+add_modelio_urdf_unit_test(ModelExporter)
 add_modelio_urdf_unit_test(URDFGenericSensorImport)
 add_modelio_urdf_unit_test(PredictSensorsMeasurement)
 add_modelio_urdf_unit_test(icubSensorURDF)
diff --git a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
new file mode 100644
index 00000000000..f5517afaa33
--- /dev/null
+++ b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2017 Fondazione Istituto Italiano di Tecnologia
+ * Authors: Silvio Traversaro
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+ *
+ */
+
+#include "testModels.h"
+
+#include <iDynTree/Core/TestUtils.h>
+
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+
+#include <cassert>
+#include <cstdio>
+#include <cstdlib>
+#include <algorithm>
+#include <fstream>
+
+using namespace iDynTree;
+
+
+void checkImportExportURDF(std::string fileName)
+{
+    // Import, export and re-import a URDF file, and check that the key properties of the model are mantained
+    ModelLoader mdlLoader;
+    bool ok = mdlLoader.loadModelFromFile(fileName, "urdf");
+    Model model = mdlLoader.model();
+    ASSERT_IS_TRUE(ok);
+
+    std::cerr << "Model loaded from " << fileName << std::endl;
+    std::ifstream original_urdf(fileName);
+
+    if (original_urdf.is_open()) {
+        std::cout << original_urdf.rdbuf();
+    }
+
+    std::string urdfString;
+    ModelExporter mdlExporter;
+    ok = mdlExporter.init(model, SensorsList());
+    ASSERT_IS_TRUE(ok);
+    ok = mdlExporter.exportModelToString(urdfString);
+    ASSERT_IS_TRUE(ok);
+
+    std::cerr << "Model serialized back to xml " << std::endl << urdfString << std::endl;
+
+    ModelLoader mdlLoaderReloaded;
+
+    ok = mdlLoaderReloaded.loadModelFromString(urdfString);
+    ASSERT_IS_TRUE(ok);
+    Model modelReloaded = mdlLoaderReloaded.model();
+
+    ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(), modelReloaded.getNrOfLinks());
+    ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(), modelReloaded.getNrOfJoints());
+    ASSERT_EQUAL_DOUBLE(model.getNrOfDOFs(), modelReloaded.getNrOfDOFs());
+
+    // TODO(traversaro) : uncomment the following line when frames are correctly handled by the exporter
+    // ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(), modelReloaded.getNrOfLinks());
+
+    // Verify that the link correspond (note that the serialization could have changed)
+    for(int lnkIndex=0; lnkIndex < model.getNrOfLinks(); lnkIndex++) {
+        LinkIndex lnkIndexInReloaded = modelReloaded.getLinkIndex(model.getLinkName(lnkIndex));
+        ASSERT_IS_TRUE(lnkIndexInReloaded != LINK_INVALID_INDEX);
+        ASSERT_IS_TRUE(model.getLinkName(lnkIndex) == modelReloaded.getLinkName(lnkIndexInReloaded));
+        SpatialInertia inertia = model.getLink(lnkIndex)->getInertia();
+        SpatialInertia inertiaReloaded = modelReloaded.getLink(lnkIndexInReloaded)->getInertia();
+        std::cerr << "Testing inertia of link " << model.getLinkName(lnkIndex) << std::endl;
+        ASSERT_EQUAL_MATRIX(inertia.asMatrix(), inertiaReloaded.asMatrix());
+    }
+
+}
+
+
+int main()
+{
+    for(unsigned int mdl = 0; mdl < IDYNTREE_TESTS_URDFS_NR; mdl++ )
+    {
+        if (std::string(IDYNTREE_TESTS_URDFS[mdl]) == "bigman.urdf")
+        {
+            // walkman model fails this test due to https://github.com/robotology/idyntree/issues/247
+            continue;
+        }
+
+        std::string urdfFileName = getAbsModelPath(std::string(IDYNTREE_TESTS_URDFS[mdl]));
+        std::cout << "Checking URDF import/export test on " << urdfFileName << std::endl;
+        checkImportExportURDF(urdfFileName);
+    }
+
+
+    return EXIT_SUCCESS;
+}

From f45ae4ae2b200710f45bf4600b1589722e67de3d Mon Sep 17 00:00:00 2001
From: Prashanth <prashanthramadoss@gmail.com>
Date: Sun, 25 Aug 2019 13:09:23 +0200
Subject: [PATCH 134/194] [Core] fix #561 - Adds left Jacobian and left
 Jacobian inverse of SO(3) to Rotation - Fixes Transform::log() - Fixes
 SpatialMotionVector::exp()

---
 doc/releases/v0_12.md                         |  3 +
 src/core/include/iDynTree/Core/Rotation.h     | 32 +++++++++
 src/core/src/Rotation.cpp                     | 66 ++++++++++++++++++-
 src/core/src/SpatialMotionVector.cpp          |  5 +-
 src/core/src/Transform.cpp                    |  8 ++-
 .../tests/TransformFromMatrix4x4UnitTest.cpp  | 19 ++++++
 6 files changed, 128 insertions(+), 5 deletions(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 7b8251d6b13..27178c5df85 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -52,6 +52,9 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)
 * Added basic tests for `Span` (https://github.com/robotology/idyntree/pull/522)
 * Implement `RPYRightTrivializedDerivativeRateOfChange()` and `RPYRightTrivializedDerivativeInverseRateOfChange()` into `Rotation` class
+* Implement left Jacobian and left Jacobian inverse of SO(3) in Rotation class (https://github.com/robotology/idyntree/pull/562)
+* Fixed implementation of Transform::log() method (https://github.com/robotology/idyntree/pull/562)
+* Fixed implementation of SpatialMotionVector::exp() method (https://github.com/robotology/idyntree/pull/562)
 
 ### `Model`
 * Implement `getTotalMass()` method for Model class
diff --git a/src/core/include/iDynTree/Core/Rotation.h b/src/core/include/iDynTree/Core/Rotation.h
index 1021dce4f24..d7afe90911c 100644
--- a/src/core/include/iDynTree/Core/Rotation.h
+++ b/src/core/include/iDynTree/Core/Rotation.h
@@ -446,6 +446,38 @@ namespace iDynTree
          */
         static Rotation RotationFromQuaternion(const iDynTree::Vector4& quaternion);
 
+        /**
+         * Get the left Jacobian of rotation matrix
+         *
+         * \f$ \omega \in \mathbb{R}^3 \f$ is the angular motion vector
+         * \f$ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) \f$ where \f$ \mathfrak{so}(3) \f$
+         *  is the set of skew symmetric matrices or the Lie algebra of \f$ SO(3) \f$
+         * \f[ J_{l_{SO(3)}} = \sum_{n = 0}^{\infty} \frac{1}{(n+1)!} [\omega_\times]^n  = (I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||^{2}} [\omega _{\times}] + \frac{||\omega|| - \text{sin}(||\omega||)}{||\omega||^{3}} [\omega _{\times}]^{2} \f]
+         *
+         * When simplified further,
+         * \f[ J_{l_{SO(3)}} = \frac{\text{sin}(||\omega||)}{||\omega||}I_3 + \frac{1 - \text{cos}(||\omega||)}{||\omega||} [\phi _{\times}] + \bigg(1 - \frac{\text{sin}(||\omega||)}{||\omega||}\bigg) \phi\phi^T \f]
+         *
+         * where \f$ \phi = \frac{\omega}{||\omega||} \f$
+         *
+         * @param[in] omega angular motion vector
+         * @return \f$ 3 \times 3 \f$ left Jacobian matrix
+         */
+        static Matrix3x3 leftJacobian(const iDynTree::AngularMotionVector3& omega);
+
+        /**
+         * Get the left Jacobian inverse of rotation matrix
+         *
+         * \f$ \omega \in \mathbb{R}^3 \f$ is the angular motion vector
+         * \f$ [\omega_\times]: \mathbb{R}^n \to \mathfrak{so}(3) \f$ where \f$ \mathfrak{so}(3) \f$
+         *  is the set of skew symmetric matrices or the Lie algebra of \f$ SO(3) \f$
+         * \f[ J^{-1} _{l _{SO(3)}} = \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) I _3 + \bigg( 1 - \frac{||\omega||}{2} \text{cot} \bigg(\frac{||\omega||}{2}\bigg) \bigg) \phi \phi^T - \frac{||\omega||}{2} [\phi _{\times}] \f]
+         *
+         * where \f$ \phi = \frac{\omega}{||\omega||} \f$
+         *
+         * @param[in] omega angular motion vector
+         * @return \f$ 3 \times 3 \f$ left Jacobian inverse matrix
+         */
+        static Matrix3x3 leftJacobianInverse(const iDynTree::AngularMotionVector3& omega);
         ///@}
 
         /** @name Output helpers.
diff --git a/src/core/src/Rotation.cpp b/src/core/src/Rotation.cpp
index c434c451542..94a4789f1a5 100644
--- a/src/core/src/Rotation.cpp
+++ b/src/core/src/Rotation.cpp
@@ -631,7 +631,71 @@ namespace iDynTree
         _rotation.fromQuaternion(_quaternion);
         return _rotation;
     }
-    
+
+    Matrix3x3 Rotation::leftJacobian(const AngularMotionVector3& omega)
+    {
+        iDynTree::Matrix3x3 J;
+        auto I3 = Eigen::MatrixXd::Identity(3, 3);
+        using iDynTree::toEigen;
+        double norm = toEigen(omega).norm();
+
+        if (iDynTree::checkDoublesAreEqual(norm, 0.0))
+        {
+            toEigen(J) = I3;
+            return J;
+        }
+
+        double c{std::cos(norm)};
+        double s{std::sin(norm)};
+
+        double alpha1{(s/norm)};
+        double alpha2{(1 - c)/norm};
+        double alpha3{(1 - (s/norm))};
+
+        Vector3 phi;
+        toEigen(phi) = toEigen(omega);
+        toEigen(phi).normalize();
+
+        Matrix3x3 phi_cross;
+        toEigen(phi_cross) = skew(toEigen(phi));
+
+        toEigen(J) = alpha1*I3 + alpha2*toEigen(phi_cross) + alpha3*toEigen(phi)*toEigen(phi).transpose();
+        return J;
+    }
+
+    Matrix3x3 Rotation::leftJacobianInverse(const AngularMotionVector3& omega)
+    {
+        iDynTree::Matrix3x3 Jinv;
+        auto I3 = Eigen::MatrixXd::Identity(3, 3);
+        using iDynTree::toEigen;
+        double norm = toEigen(omega).norm();
+
+        if (iDynTree::checkDoublesAreEqual(norm, 0.0))
+        {
+            toEigen(Jinv) = I3;
+            return Jinv;
+        }
+
+        double normovertwo{norm/2.0};
+        double c{std::cos(normovertwo)};
+        double s{std::sin(normovertwo)};
+        double cot{c/s};
+
+        double alpha1{(normovertwo*cot)};
+        double alpha2{(-normovertwo)};
+        double alpha3{(1 - alpha1)};
+
+        Vector3 phi;
+        toEigen(phi) = toEigen(omega);
+        toEigen(phi).normalize();
+
+        Matrix3x3 phi_cross;
+        toEigen(phi_cross) = skew(toEigen(phi));
+
+        toEigen(Jinv) = alpha1*I3  + alpha2*toEigen(phi_cross) + alpha3*toEigen(phi)*toEigen(phi).transpose();
+        return Jinv;
+    }
+
     std::string Rotation::toString() const
     {
         std::stringstream ss;
diff --git a/src/core/src/SpatialMotionVector.cpp b/src/core/src/SpatialMotionVector.cpp
index 8971769c6a5..0b45c5253da 100644
--- a/src/core/src/SpatialMotionVector.cpp
+++ b/src/core/src/SpatialMotionVector.cpp
@@ -97,9 +97,10 @@ Transform SpatialMotionVector::exp() const
     // Understand if there is a meaningful
     // semantics for this operation and if it exists use it
 
-    // the linear part is not changed by the exp
+    // the linear part is affected by the left Jacobian of SO(3)
     Position newPos;
-    memcpy(newPos.data(),this->getLinearVec3().data(),3*sizeof(double));
+    auto J_SO3 = Rotation::leftJacobian(this->getAngularVec3());
+    toEigen(newPos) = toEigen(J_SO3)*toEigen(this->getLinearVec3());
     res.setPosition(newPos);
 
     // the angular part instead mapped by so(3) -> SO(3) exp map
diff --git a/src/core/src/Transform.cpp b/src/core/src/Transform.cpp
index 436129a28f5..943a463ba18 100644
--- a/src/core/src/Transform.cpp
+++ b/src/core/src/Transform.cpp
@@ -531,8 +531,12 @@ std::string Transform::reservedToString() const
         // Understand if there is a meaningful
         // semantics for this operation and if it exists use it
 
-        // the linear part is not changed by the log
-        memcpy(logRes.getLinearVec3().data(),this->getPosition().data(),3*sizeof(double));
+        // the linear part is affected by the left Jacobian inverse of SO(3)
+        auto omega = this->getRotation().log();
+        auto JinvSO3 = Rotation::leftJacobianInverse(omega);
+        Vector3 rho;
+        toEigen(rho) = toEigen(JinvSO3)*toEigen(this->getPosition());
+        memcpy(logRes.getLinearVec3().data(),rho.data(),3*sizeof(double));
 
         // the angular part instead mapped by SO(3) -> so(3) log
         logRes.setAngularVec3(this->getRotation().log());
diff --git a/src/core/tests/TransformFromMatrix4x4UnitTest.cpp b/src/core/tests/TransformFromMatrix4x4UnitTest.cpp
index 600d66a70a2..c5d405c9b3f 100644
--- a/src/core/tests/TransformFromMatrix4x4UnitTest.cpp
+++ b/src/core/tests/TransformFromMatrix4x4UnitTest.cpp
@@ -12,6 +12,7 @@
 #include <iDynTree/Core/Transform.h>
 #include <iDynTree/Core/Position.h>
 #include <iDynTree/Core/Rotation.h>
+#include <iDynTree/Core/SpatialMotionVector.h>
 #include <iDynTree/Core/MatrixFixSize.h>
 #include <iostream>
 #include <string>
@@ -36,8 +37,26 @@ void checkFromMatrix4x4()
     ASSERT_EQUAL_TRANSFORM(t_posrot, t_buffer);
 }
 
+void checkLogExp()
+{
+    PositionRaw pos_raw(0, 0, 0);
+    Rotation rot = Rotation::RPY(0.0, 1.57, 0.0);
+    Position pos = Position(pos_raw);
+    Transform t_posrot(rot, pos);
+
+    SpatialMotionVector v;
+    v.zero();
+    v.setAngularVec3(rot.log());
+
+    Transform t_v = v.exp();
+
+    ASSERT_EQUAL_TRANSFORM_TOL(t_posrot, t_v, 0.01);
+}
+
+
 int main()
 {
     checkFromMatrix4x4();
+    checkLogExp();
     return EXIT_SUCCESS;
 }

From 62a0bbf4f967abd34803a3e5219cc45b22c74992 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 19 Aug 2019 18:03:38 +0200
Subject: [PATCH 135/194] Add Model::getLinkAdditionalFrames method

To return all the additional frames connected to a given link.
---
 src/model/include/iDynTree/Model/Model.h | 12 ++++++++++++
 src/model/src/Model.cpp                  | 23 +++++++++++++++++++++++
 src/model/tests/ModelUnitTest.cpp        | 12 ++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/src/model/include/iDynTree/Model/Model.h b/src/model/include/iDynTree/Model/Model.h
index 7a07538de33..067ad8dda90 100644
--- a/src/model/include/iDynTree/Model/Model.h
+++ b/src/model/include/iDynTree/Model/Model.h
@@ -417,6 +417,18 @@ namespace iDynTree
          */
         LinkIndex getFrameLink(const FrameIndex frameIndex) const;
 
+        /**
+         * Get the additional frames of a specified link.
+         *
+         * @note The vector of returned frame index is ordered according to the frame index.
+         * @warning This method searches linearly over all the frames.
+         *
+         * @param[in] link a LinkIndex of the specified link,
+         * @param[out] frames a vector of FrameIndex of the frame indeces attached to the specified link index,
+         * @return true if the specified link is a valid link, false otherwise.
+         */
+        bool getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector<FrameIndex>& frameIndeces) const;
+
         /**
          * Get the nr of neighbors of a given link.
          */
diff --git a/src/model/src/Model.cpp b/src/model/src/Model.cpp
index e1cb29852b3..41e30afb423 100644
--- a/src/model/src/Model.cpp
+++ b/src/model/src/Model.cpp
@@ -703,6 +703,29 @@ LinkIndex Model::getFrameLink(const FrameIndex frameIndex) const
     return LINK_INVALID_INDEX;
 }
 
+bool Model::getLinkAdditionalFrames(const LinkIndex lnkIndex, std::vector<FrameIndex>& frameIndices) const
+{
+    if (!isValidLinkIndex(lnkIndex)) {
+        std::stringstream ss;
+        ss << "LinkIndex " << lnkIndex << " is not valid, should be between 0 and " << this->getNrOfLinks()-1;
+        reportError("Model", "getLinkAdditionalFrames", ss.str().c_str());
+        return false;
+    }
+
+    frameIndices.resize(0);
+    // FrameIndex from 0 to this->getNrOfLinks()-1 are reserved for implicit frame of Links
+    // with the corresponding LinkIndex, while the frameIndex from getNrOfLinks() to getNrOfFrames()-1
+    // are the one of actual additional frames. See iDynTree::Model docs for more details
+    for (FrameIndex frameIndex=this->getNrOfLinks(); frameIndex < this->getNrOfFrames(); frameIndex++) {
+        if (this->getFrameLink(frameIndex) == lnkIndex) {
+            frameIndices.push_back(frameIndex);
+        }
+    }
+
+    return true;
+}
+
+
 
 unsigned int Model::getNrOfNeighbors(const LinkIndex link) const
 {
diff --git a/src/model/tests/ModelUnitTest.cpp b/src/model/tests/ModelUnitTest.cpp
index 501a5a8af53..7be13829ae5 100644
--- a/src/model/tests/ModelUnitTest.cpp
+++ b/src/model/tests/ModelUnitTest.cpp
@@ -289,12 +289,24 @@ void checkSimpleModel()
 
         model.addJoint("fixedJoint",&fixJoint);
 
+        model.addAdditionalFrameToLink("link0", "frame0_0", iDynTree::Transform::Identity());
+        model.addAdditionalFrameToLink("link1", "frame1_0", iDynTree::Transform::Identity());
+        model.addAdditionalFrameToLink("link1", "frame1_1", iDynTree::Transform::Identity());
+
+
         ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(),2);
         ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(),1);
         ASSERT_EQUAL_DOUBLE(model.getNrOfNeighbors(0),1);
         ASSERT_EQUAL_DOUBLE(model.getNrOfNeighbors(1),1);
         ASSERT_EQUAL_DOUBLE(model.getNeighbor(0,0).neighborLink,1);
         ASSERT_EQUAL_DOUBLE(model.getNeighbor(1,0).neighborLink,0);
+        std::vector<FrameIndex> link0_frames;
+        model.getLinkAdditionalFrames(0, link0_frames);
+        ASSERT_EQUAL_DOUBLE(link0_frames.size(), 1);
+        std::vector<FrameIndex> link1_frames;
+        model.getLinkAdditionalFrames(1, link1_frames);
+        ASSERT_EQUAL_DOUBLE(link1_frames.size(), 2);
+        ASSERT_IS_TRUE(link1_frames[0] < link1_frames[1]);
 
         createCopyAndDestroy(model);
         checkComputeTraversal(model);

From 358beda0c8ec74000f0778cdb602dafb5347d20b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 19 Aug 2019 18:04:32 +0200
Subject: [PATCH 136/194] Add support for exporting additional frames in the
 ModelExporter's URDF support

---
 .../include/iDynTree/ModelIO/ModelExporter.h  |  21 +++-
 src/model_io/urdf/src/URDFModelExport.cpp     | 100 +++++++++++++++++-
 .../urdf/tests/ModelExporterUnitTest.cpp      |  14 ++-
 3 files changed, 130 insertions(+), 5 deletions(-)

diff --git a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
index 2a6b910515f..9fb5195e416 100644
--- a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
+++ b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
@@ -63,11 +63,30 @@ class ModelExporterOptions
  *
  * Furthermore, currently the model exporter only exports a subset of the features
  * supported in iDynTree::Model. In particular, the following features are not exported:
- * * Additional frames
+ *
  * * Visual and collision geometries
  * * Sensors
  * * Joint limits, damping and static friction.
  *
+ * # Format documentation
+ *
+ * The following format are  supported  by the exporter.
+ *
+ * | Format | Extendend Name | Website |  String for filetype argument  |
+ * |:-----------------------------:|:-------:|:-------:|:--------:|
+ * | URDF | Unified Robot Description Format  | http://wiki.ros.org/urdf | `urdf` |
+ *
+ * ## URDF
+ *
+ * As the URDF format does not distinguish between frames and links (see https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56
+ * for an extensive discussion on this) the URDF model exporter converts iDynTree's *additional frames* to mass-less and shape-less
+ * *fake* URDF links that are connected as child links via `fixed` joints to the corresponding **real** URDF links.
+ *
+ * Furthermore, it is widespread use in URDF models to never use a real link (with mass) as the root link of a model, mainly
+ * due to workaround a bug in official %KDL parser used in ROS (see https://github.com/ros/kdl_parser/issues/27 for more info). For this reason,
+ * if the selected base_link has at least one additional frame, the first additional frame of the base link is added as a **parent** fake URDF link,
+ * instead as a **child** fake URDF link as done with the rest of %iDynTree's additional frames. If no additional frame is available for the base link,
+ * the base link of the URDF will have a mass, and will generate a warning then used with the ROS's [`kdl_parser`](https://github.com/ros/kdl_parser) .
  *
  */
 class ModelExporter
diff --git a/src/model_io/urdf/src/URDFModelExport.cpp b/src/model_io/urdf/src/URDFModelExport.cpp
index 417ed718f6a..05c7603c7a1 100644
--- a/src/model_io/urdf/src/URDFModelExport.cpp
+++ b/src/model_io/urdf/src/URDFModelExport.cpp
@@ -29,6 +29,15 @@
 namespace iDynTree
 {
 
+/*
+ * This file uses extensivly the libxml2's tree API, see
+ * http://www.xmlsoft.org/examples/index.html#Tree for more info.
+ * Usage of the C-based libxml2 API in C++ means that is necessary to
+ * use the BAD_CAST macro for handling const-correctness.
+ * See https://stackoverflow.com/questions/45058878/why-libxml2-uses-bad-cast-everywhere-in-a-c-c-code
+ * and http://www.xmlsoft.org/html/libxml-xmlstring.html#BAD_CAST for details on this.
+ */
+
 /*
  * Add a <origin> URDF element to the specified parent element with the specified transform.
  *
@@ -133,6 +142,7 @@ bool exportLink(const Link &link, const std::string linkName, xmlNodePtr parent_
     return ok;
 }
 
+
 /**
  * Add a <joint> URDF element to the specified parent element with the specified joint info.
  *
@@ -241,6 +251,68 @@ bool exportJoint(IJointConstPtr joint, LinkConstPtr parentLink, LinkConstPtr chi
     return ok;
 }
 
+/*
+ * Add a <link> and <joint> URDF elements to the specified parent element for the specified additional frame.
+ *
+ * URDF does not have the concept of frame (see https://discourse.ros.org/t/urdf-ng-link-and-frame-concepts/56),
+ * so for each additional frame we need to add a URDF "fake" mass-less link.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
+ *
+ * <parent_element>
+ *   <link name="<frame_name>"/>
+ *   <joint name="<frame_name>_fixed_joint" type="fixed">
+ *      <origin xyz="0.1 0.2 0.3" rpy="0 0 3.1416"/>
+ *      <parent link="<link_name>"/>
+ *      <child link="<frame_name>"/>
+ *   </joint>
+ * <parent_element/>
+ *
+ * where the actual values of the added element and attributes depend on the input frame_name, link_H_frame, parent_link_name and direction_options arguments.
+ */
+enum exportAdditionalFrameDirectionOption {
+    FAKE_LINK_IS_CHILD,
+    FAKE_LINK_IS_PARENT
+};
+bool exportAdditionalFrame(const std::string frame_name, Transform link_H_frame, const std::string link_name, exportAdditionalFrameDirectionOption direction_option, xmlNodePtr parent_element)
+{
+    bool ok=true;
+
+    // Export fake link
+    xmlNodePtr link_xml = xmlNewChild(parent_element, NULL, BAD_CAST "link", NULL);
+    xmlNewProp(link_xml, BAD_CAST "name", BAD_CAST frame_name.c_str());
+
+    // Export fake joint
+    xmlNodePtr joint_xml = xmlNewChild(parent_element, NULL, BAD_CAST "joint", NULL);
+    std::string fake_joint_name = frame_name + "_fixed_joint";
+    xmlNewProp(joint_xml, BAD_CAST "name", BAD_CAST fake_joint_name.c_str());
+    xmlNewProp(joint_xml, BAD_CAST "type", BAD_CAST "fixed");
+
+    // origin
+    exportTransform(link_H_frame, joint_xml);
+
+    std::string parent_of_fake_joint, child_of_fake_joint;
+
+    if (direction_option == FAKE_LINK_IS_CHILD) {
+        parent_of_fake_joint = link_name;
+        child_of_fake_joint = frame_name;
+    } else {
+        // direction_option == FAKE_LINK_IS_PARENT
+        parent_of_fake_joint = frame_name;
+        child_of_fake_joint = link_name;
+    }
+
+    // parent
+    xmlNodePtr parent_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "parent", NULL);
+    xmlNewProp(parent_xml, BAD_CAST "link", BAD_CAST parent_of_fake_joint.c_str());
+
+    // child
+    xmlNodePtr child_xml = xmlNewChild(joint_xml, NULL, BAD_CAST "child", NULL);
+    xmlNewProp(child_xml, BAD_CAST "link", BAD_CAST child_of_fake_joint.c_str());
+
+    return ok;
+}
+
 bool URDFStringFromModel(const iDynTree::Model & model,
                          std::string & urdf_string,
                          const ModelExporterOptions options)
@@ -272,6 +344,21 @@ bool URDFStringFromModel(const iDynTree::Model & model,
         return false;
     }
 
+    // If the base link has at least an additional frame, add it as parent URDF link
+    // as a workaround for https://github.com/ros/kdl_parser/issues/27
+    FrameIndex baseFakeLinkFrameIndex = FRAME_INVALID_INDEX;
+    std::vector<FrameIndex> frameIndices;
+    ok = model.getLinkAdditionalFrames(baseLinkIndex, frameIndices);
+    if (ok && frameIndices.size() >= 1) {
+        baseFakeLinkFrameIndex = frameIndices[0];
+        ok = ok && exportAdditionalFrame(model.getFrameName(baseFakeLinkFrameIndex),
+                                         model.getFrameTransform(baseFakeLinkFrameIndex),
+                                         model.getLinkName(model.getFrameLink(baseFakeLinkFrameIndex)),
+                                         FAKE_LINK_IS_PARENT,
+                                         robot);
+    }
+
+
     // Create a Traversal
     Traversal exportTraversal;
     if (!model.computeFullTreeTraversal(exportTraversal, baseLinkIndex))
@@ -300,7 +387,18 @@ bool URDFStringFromModel(const iDynTree::Model & model,
         ok = ok && exportLink(*visitedLink, visitedLinkName, robot);
     }
 
-    // TODO(traversaro) : export additional frames
+    // Export all the additional frames that are not parent of the base link
+    for (FrameIndex frameIndex=model.getNrOfLinks(); frameIndex < static_cast<FrameIndex>(model.getNrOfFrames()); frameIndex++)
+    {
+        if (frameIndex != baseFakeLinkFrameIndex) {
+            ok = ok && exportAdditionalFrame(model.getFrameName(frameIndex),
+                                             model.getFrameTransform(frameIndex),
+                                             model.getLinkName(model.getFrameLink(frameIndex)),
+                                             FAKE_LINK_IS_CHILD,
+                                             robot);
+        }
+    }
+
 
     xmlChar *xmlbuff=0;
     int buffersize=0;
diff --git a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
index f5517afaa33..a827105f5cd 100644
--- a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
+++ b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
@@ -57,9 +57,7 @@ void checkImportExportURDF(std::string fileName)
     ASSERT_EQUAL_DOUBLE(model.getNrOfLinks(), modelReloaded.getNrOfLinks());
     ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(), modelReloaded.getNrOfJoints());
     ASSERT_EQUAL_DOUBLE(model.getNrOfDOFs(), modelReloaded.getNrOfDOFs());
-
-    // TODO(traversaro) : uncomment the following line when frames are correctly handled by the exporter
-    // ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(), modelReloaded.getNrOfLinks());
+    ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(), modelReloaded.getNrOfFrames());
 
     // Verify that the link correspond (note that the serialization could have changed)
     for(int lnkIndex=0; lnkIndex < model.getNrOfLinks(); lnkIndex++) {
@@ -72,6 +70,16 @@ void checkImportExportURDF(std::string fileName)
         ASSERT_EQUAL_MATRIX(inertia.asMatrix(), inertiaReloaded.asMatrix());
     }
 
+    // Verify that the frame correspond (note that the serialization could have changed)
+    for(FrameIndex frameIndex=model.getNrOfLinks(); frameIndex < model.getNrOfFrames(); frameIndex++) {
+        FrameIndex frameIndexInReloaded = modelReloaded.getFrameIndex(model.getFrameName(frameIndex));
+        ASSERT_IS_TRUE(frameIndexInReloaded != FRAME_INVALID_INDEX);
+        ASSERT_IS_TRUE(model.getFrameName(frameIndex) == modelReloaded.getFrameName(frameIndexInReloaded));
+        Transform link_H_frame = model.getFrameTransform(frameIndex);
+        Transform link_H_frame_reloaded = modelReloaded.getFrameTransform(frameIndexInReloaded);
+        ASSERT_EQUAL_MATRIX(link_H_frame.asHomogeneousTransform(), link_H_frame_reloaded.asHomogeneousTransform());
+    }
+
 }
 
 

From 18ce08cd22bc6d4848def3efc190e968bd18df3b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 26 Aug 2019 14:25:42 +0200
Subject: [PATCH 137/194] Add nameIsValid attribute to iDynTree::SolidShape
 class

---
 doc/releases/v0_12.md                          | 1 +
 src/model/include/iDynTree/Model/SolidShapes.h | 4 ++++
 2 files changed, 5 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 6be463b712e..4ac0f11f0ce 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -56,6 +56,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Implement left Jacobian and left Jacobian inverse of SO(3) in Rotation class (https://github.com/robotology/idyntree/pull/562)
 * Fixed implementation of Transform::log() method (https://github.com/robotology/idyntree/pull/562)
 * Fixed implementation of SpatialMotionVector::exp() method (https://github.com/robotology/idyntree/pull/562)
+* Add nameIsValid attribute to iDynTree::SolidShape class.
 
 ### `Model`
 * Implement `getTotalMass()` method for Model class
diff --git a/src/model/include/iDynTree/Model/SolidShapes.h b/src/model/include/iDynTree/Model/SolidShapes.h
index 9ea4ba27962..0560e484ef3 100644
--- a/src/model/include/iDynTree/Model/SolidShapes.h
+++ b/src/model/include/iDynTree/Model/SolidShapes.h
@@ -30,6 +30,10 @@ namespace iDynTree
         virtual ~SolidShape()=0;
         virtual SolidShape* clone()=0;
         std::string name;
+        /**
+         * True if the name is valid, false otherwise.
+         */
+        bool nameIsValid{false};
         Transform link_H_geometry;
 
         /**

From e0c4eda135d6991f63bda0e27334b0ec74a78b40 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 26 Aug 2019 14:28:38 +0200
Subject: [PATCH 138/194] Correctly parse if option collision or visual name is
 present in URDF file

---
 doc/releases/v0_12.md                             | 1 +
 src/model_io/urdf/include/private/VisualElement.h | 1 +
 src/model_io/urdf/src/URDFDocument.cpp            | 1 +
 src/model_io/urdf/src/VisualElement.cpp           | 4 ++++
 4 files changed, 7 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 4ac0f11f0ce..bfb9d223e24 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -63,6 +63,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 
 ### `modelio`
 * Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
+* Added support in the URDF parser to correctly parse the optional name parameter of visual and collision elements.
 
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
diff --git a/src/model_io/urdf/include/private/VisualElement.h b/src/model_io/urdf/include/private/VisualElement.h
index 6e88c548dd6..94e3cd9e8ea 100644
--- a/src/model_io/urdf/include/private/VisualElement.h
+++ b/src/model_io/urdf/include/private/VisualElement.h
@@ -36,6 +36,7 @@ class iDynTree::VisualElement: public iDynTree::XMLElement
 public:
     struct VisualInfo {
         std::string m_name;
+        bool m_nameAttributeFound;
         iDynTree::Transform m_origin{iDynTree::Transform::Identity()};
         std::shared_ptr<SolidShape> m_solidShape;
         std::shared_ptr<MaterialElement::MaterialInfo> m_material;
diff --git a/src/model_io/urdf/src/URDFDocument.cpp b/src/model_io/urdf/src/URDFDocument.cpp
index 34885620050..adf60730dad 100644
--- a/src/model_io/urdf/src/URDFDocument.cpp
+++ b/src/model_io/urdf/src/URDFDocument.cpp
@@ -481,6 +481,7 @@ namespace iDynTree {
                 }
 
                 shape->name = visual.m_name;
+                shape->nameIsValid = visual.m_nameAttributeFound;
                 shape->link_H_geometry = link_H_geometry;
 
                 LinkIndex idyntreeLinkIndex = model.getLinkIndex(linkName);
diff --git a/src/model_io/urdf/src/VisualElement.cpp b/src/model_io/urdf/src/VisualElement.cpp
index 5e5f90fcab4..152382ca7ca 100644
--- a/src/model_io/urdf/src/VisualElement.cpp
+++ b/src/model_io/urdf/src/VisualElement.cpp
@@ -34,6 +34,10 @@ namespace iDynTree {
         auto found = attributes.find("name");
         if (found != attributes.end()) {
             m_info.m_name = found->second->value();
+            m_info.m_nameAttributeFound = true;
+        } else {
+            m_info.m_name = "";
+            m_info.m_nameAttributeFound = false;
         }
         return true;
     }

From 02528556cb1b9263198a1c7c5d3188252dbb1ca6 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 27 Aug 2019 18:43:56 +0200
Subject: [PATCH 139/194] Add exportFirstBaseLinkAdditionalFrameAsFakeURDFBase
 option to ModelExporter (#567)

* Add exportFirstBaseLinkAdditionalFrameAsFakeURDFBase option to ModelExporter

* Update URDFModelExport.cpp
---
 .../include/iDynTree/ModelIO/ModelExporter.h     | 16 +++++++++++++++-
 src/model_io/urdf/src/URDFModelExport.cpp        |  9 +++++++--
 2 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
index 9fb5195e416..d93960ed6f2 100644
--- a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
+++ b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
@@ -42,6 +42,19 @@ class ModelExporterOptions
      */
     std::string baseLink;
 
+    /**
+     * Select if the first additional frame of the base link is exported as fake base link.
+     *
+     * The URDF exporter by default exports the first additional frame of the base link as
+     * a parent "fake" link to the actual base link, as a workaround for https://github.com/ros/kdl_parser/issues/27).
+     * By setting this option to false, is possible to disable this behaviour, for more info see iDynTree::ModelExporter docs.
+     * This option is ignored in non-URDF exporter.
+     *
+     * Default value: true.
+     * Supported formats: urdf.
+     */
+    bool exportFirstBaseLinkAdditionalFrameAsFakeURDFBase{true};
+
     /**
      * Constructor.
      */
@@ -84,9 +97,10 @@ class ModelExporterOptions
  *
  * Furthermore, it is widespread use in URDF models to never use a real link (with mass) as the root link of a model, mainly
  * due to workaround a bug in official %KDL parser used in ROS (see https://github.com/ros/kdl_parser/issues/27 for more info). For this reason,
- * if the selected base_link has at least one additional frame, the first additional frame of the base link is added as a **parent** fake URDF link,
+ * if the selected base_link has at least one additional frame, by default the first additional frame of the base link is added as a **parent** fake URDF link,
  * instead as a **child** fake URDF link as done with the rest of %iDynTree's additional frames. If no additional frame is available for the base link,
  * the base link of the URDF will have a mass, and will generate a warning then used with the ROS's [`kdl_parser`](https://github.com/ros/kdl_parser) .
+ * This behaviour can be disabled by setting to false the `exportFirstBaseLinkAdditionalFrameAsFakeURDFBase` attribute of ModelExporterOptions.
  *
  */
 class ModelExporter
diff --git a/src/model_io/urdf/src/URDFModelExport.cpp b/src/model_io/urdf/src/URDFModelExport.cpp
index 05c7603c7a1..5cdba6df77c 100644
--- a/src/model_io/urdf/src/URDFModelExport.cpp
+++ b/src/model_io/urdf/src/URDFModelExport.cpp
@@ -345,11 +345,16 @@ bool URDFStringFromModel(const iDynTree::Model & model,
     }
 
     // If the base link has at least an additional frame, add it as parent URDF link
-    // as a workaround for https://github.com/ros/kdl_parser/issues/27
+    // as a workaround for https://github.com/ros/kdl_parser/issues/27, unless
+    // options.exportFirstBaseLinkAdditionalFrameAsFakeURDFBase is set to false
+    // If options.exportFirstBaseLinkAdditionalFrameAsFakeURDFBase is set to false,
+    // baseFakeLinkFrameIndex remains set to FRAME_INVALID_INDEX, a
+    // and all the additional frames of the base link get exported as child fake links 
+    // in the loop and the end of this function
     FrameIndex baseFakeLinkFrameIndex = FRAME_INVALID_INDEX;
     std::vector<FrameIndex> frameIndices;
     ok = model.getLinkAdditionalFrames(baseLinkIndex, frameIndices);
-    if (ok && frameIndices.size() >= 1) {
+    if (ok && frameIndices.size() >= 1 && options.exportFirstBaseLinkAdditionalFrameAsFakeURDFBase) {
         baseFakeLinkFrameIndex = frameIndices[0];
         ok = ok && exportAdditionalFrame(model.getFrameName(baseFakeLinkFrameIndex),
                                          model.getFrameTransform(baseFakeLinkFrameIndex),

From a2e16d7b5235e5c8ffaf5348a6edb9ff15582b4d Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 2 Sep 2019 14:18:04 +0200
Subject: [PATCH 140/194] Fix missing includes in WorhpInterface.h

---
 .../include/iDynTree/Optimizers/WorhpInterface.h               | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h b/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
index e7272e5e6fe..2d4dcc73b4b 100644
--- a/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
+++ b/src/optimalcontrol/include/iDynTree/Optimizers/WorhpInterface.h
@@ -19,6 +19,9 @@
 
 #include <iDynTree/Optimizer.h>
 
+#include <memory>
+#include <string>
+
 namespace iDynTree {
 
     class VectorDynSize;

From 1539881c0d02f299ab1d9c20acc07442a72fd052 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 26 Aug 2019 14:29:52 +0200
Subject: [PATCH 141/194] Add support in ModelExporter to export visual and
 collision geometries to URDF

---
 .../include/iDynTree/ModelIO/ModelExporter.h  |   1 -
 src/model_io/urdf/src/URDFModelExport.cpp     | 142 ++++++++++++++++--
 .../urdf/tests/ModelExporterUnitTest.cpp      |  95 +++++++++++-
 3 files changed, 220 insertions(+), 18 deletions(-)

diff --git a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
index 9fb5195e416..67a2fbec9f9 100644
--- a/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
+++ b/src/model_io/urdf/include/iDynTree/ModelIO/ModelExporter.h
@@ -64,7 +64,6 @@ class ModelExporterOptions
  * Furthermore, currently the model exporter only exports a subset of the features
  * supported in iDynTree::Model. In particular, the following features are not exported:
  *
- * * Visual and collision geometries
  * * Sensors
  * * Joint limits, damping and static friction.
  *
diff --git a/src/model_io/urdf/src/URDFModelExport.cpp b/src/model_io/urdf/src/URDFModelExport.cpp
index 05c7603c7a1..d819b53c990 100644
--- a/src/model_io/urdf/src/URDFModelExport.cpp
+++ b/src/model_io/urdf/src/URDFModelExport.cpp
@@ -106,10 +106,122 @@ bool exportInertial(const SpatialInertia &inertia, xmlNodePtr parent_element)
     return ok;
 }
 
+
+
+/*
+ * Add a <visual> or <collision> URDF element to the specified parent element for the specified solid shapes.
+ *
+ * URDF support <box>, <cylinder>, <sphere> and <mesh> elements as shapes.
+ *
+ * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
+ * <parent_element>
+ *   <visual>
+ *     <origin xyz="0 0 0" rpy="0 0 0" />
+ *     <geometry>
+ *       <box size="1 1 1" />
+ *     </geometry>
+ *     <material name="Cyan">
+ *       <color rgba="0 1.0 1.0 1.0"/>
+ *     </material>
+ *   </visual>
+ * <parent_element/>
+ * where the actual values of the added element and attributes depend on the input solidShape.
+ */
+enum exportSolidShapePropertyType {
+    VISUAL,
+    COLLISION
+};
+bool exportSolidShape(const SolidShape* solidShape, exportSolidShapePropertyType type, xmlNodePtr parent_element)
+{
+    bool ok=true;
+    std::string element_name;
+    if (type == VISUAL) {
+        element_name = "visual";
+    } else {
+        // assert(type == COLLISION)
+        element_name = "collision";
+    }
+
+    xmlNodePtr root_shape_xml = xmlNewChild(parent_element, NULL, BAD_CAST element_name.c_str(), NULL);
+    // Omit the name (that is an optional attribute) if the shape has no name
+    if (solidShape->nameIsValid) {
+        std::string shapeName = solidShape->name;
+        xmlNewProp(root_shape_xml, BAD_CAST "name", BAD_CAST shapeName.c_str());
+    }
+
+    // Export transform
+    ok = ok && exportTransform(solidShape->link_H_geometry, root_shape_xml);
+
+    // Export geometry
+    xmlNodePtr geometry_xml = xmlNewChild(root_shape_xml, NULL, BAD_CAST "geometry", NULL);
+
+    if (solidShape->isBox()) {
+        const Box* box = solidShape->asBox();
+
+        // Export box element
+        xmlNodePtr box_xml = xmlNewChild(geometry_xml, NULL, BAD_CAST "box", NULL);
+
+        // Export size attribute
+        double size_data[3] = {box->x, box->y, box->z};
+        std::string size_str;
+        ok = ok && vectorToString(Vector3(size_data, 3) , size_str);
+        xmlNewProp(box_xml, BAD_CAST "size", BAD_CAST size_str.c_str());
+
+    } else if (solidShape->isCylinder()) {
+        const Cylinder* cylinder = solidShape->asCylinder();
+
+        // Export cylinder element
+        xmlNodePtr cylinder_xml = xmlNewChild(geometry_xml, NULL, BAD_CAST "cylinder", NULL);
+
+        // Export radius attribute
+        std::string radius_str;
+        ok = ok && doubleToStringWithClassicLocale(cylinder->radius, radius_str);
+        xmlNewProp(cylinder_xml, BAD_CAST "radius", BAD_CAST radius_str.c_str());
+
+        // Export length attribute
+        std::string length_str;
+        ok = ok && doubleToStringWithClassicLocale(cylinder->length, length_str);
+        xmlNewProp(cylinder_xml, BAD_CAST "length", BAD_CAST length_str.c_str());
+
+    } else if (solidShape->isSphere()) {
+        const Sphere* sphere = solidShape->asSphere();
+
+        // Export sphere  element
+        xmlNodePtr sphere_xml = xmlNewChild(geometry_xml, NULL, BAD_CAST "sphere", NULL);
+
+        // Export radius attribute
+        std::string radius_str;
+        ok = ok && doubleToStringWithClassicLocale(sphere->radius, radius_str);
+        xmlNewProp(sphere_xml, BAD_CAST "radius", BAD_CAST radius_str.c_str());
+
+    } else if (solidShape->isExternalMesh()) {
+        const ExternalMesh* mesh = solidShape->asExternalMesh();
+
+        // Export mesh element
+        xmlNodePtr mesh_xml = xmlNewChild(geometry_xml, NULL, BAD_CAST "mesh", NULL);
+
+        // Export filename attribute
+        xmlNewProp(mesh_xml, BAD_CAST "filename", BAD_CAST mesh->filename.c_str());
+
+        // Export scale attribute
+        std::string scale_str;
+        ok = ok && vectorToString(mesh->scale, scale_str);
+        xmlNewProp(mesh_xml, BAD_CAST "scale", BAD_CAST scale_str.c_str());
+
+    } else {
+        std::cerr << "[ERROR] URDFModelExport: Impossible to convert geometry of type "
+                  <<  typeid(solidShape).name() << " to a URDF geometry." << std::endl;
+        return false;
+    }
+
+    return ok;
+
+    return ok;
+}
+
 /*
  * Add a <link> URDF element to the specified parent element with the specified link info.
  *
- * Currently this function does not export visual and collision elements.
  *
  * If parent_element contains a pointer to a tag <parent_element></parent_element>, this function modifies it to be something like:
  *
@@ -124,7 +236,7 @@ bool exportInertial(const SpatialInertia &inertia, xmlNodePtr parent_element)
  *
  * where the actual values of the added element and attributes depend on the input link and linkName arguments.
  */
-bool exportLink(const Link &link, const std::string linkName, xmlNodePtr parent_element)
+bool exportLink(const Link &link, const std::string linkName, const Model& model, xmlNodePtr parent_element)
 {
     bool ok = true;
     xmlNodePtr link_xml = xmlNewChild(parent_element, NULL, BAD_CAST "link", NULL);
@@ -132,12 +244,20 @@ bool exportLink(const Link &link, const std::string linkName, xmlNodePtr parent_
 
     ok = ok && exportInertial(link.getInertia(), link_xml);
 
-    /* TODO(traversaro) : support solid shapes
-    for (std::size_t i = 0 ; i < link.visual_array.size() ; ++i)
-        exportVisual(*link.visual_array[i], link_xml);
-    for (std::size_t i = 0 ; i < link.collision_array.size() ; ++i)
-        exportCollision(*link.collision_array[i], link_xml);
-    */
+    LinkIndex linkIndex = model.getLinkIndex(linkName);
+
+    // Export visual shapes
+    for(unsigned int shapeIdx=0; shapeIdx < model.visualSolidShapes().linkSolidShapes[linkIndex].size(); shapeIdx++) {
+        SolidShape * exportedShape = model.visualSolidShapes().linkSolidShapes[linkIndex][shapeIdx];
+        exportSolidShape(exportedShape, VISUAL, link_xml);
+    }
+
+    // Export collision shapes
+    for(unsigned int shapeIdx=0; shapeIdx < model.collisionSolidShapes().linkSolidShapes[linkIndex].size(); shapeIdx++) {
+        // Clone the shape
+        SolidShape * exportedShape = model.collisionSolidShapes().linkSolidShapes[linkIndex][shapeIdx];
+        exportSolidShape(exportedShape, COLLISION, link_xml);
+    }
 
     return ok;
 }
@@ -313,6 +433,9 @@ bool exportAdditionalFrame(const std::string frame_name, Transform link_H_frame,
     return ok;
 }
 
+
+
+
 bool URDFStringFromModel(const iDynTree::Model & model,
                          std::string & urdf_string,
                          const ModelExporterOptions options)
@@ -384,7 +507,7 @@ bool URDFStringFromModel(const iDynTree::Model & model,
         }
 
         // Export link
-        ok = ok && exportLink(*visitedLink, visitedLinkName, robot);
+        ok = ok && exportLink(*visitedLink, visitedLinkName, model, robot);
     }
 
     // Export all the additional frames that are not parent of the base link
@@ -399,7 +522,6 @@ bool URDFStringFromModel(const iDynTree::Model & model,
         }
     }
 
-
     xmlChar *xmlbuff=0;
     int buffersize=0;
     xmlDocDumpFormatMemory(urdf_xml, &xmlbuff, &buffersize, 1);
diff --git a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
index a827105f5cd..36e1da7907f 100644
--- a/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
+++ b/src/model_io/urdf/tests/ModelExporterUnitTest.cpp
@@ -23,6 +23,65 @@
 
 using namespace iDynTree;
 
+unsigned int getNrOfVisuals(const iDynTree::Model& model)
+{
+    unsigned int nrOfVisuals = 0;
+    for (LinkIndex index = 0; index < model.getNrOfLinks(); ++index) {
+        nrOfVisuals += model.visualSolidShapes().linkSolidShapes[index].size();
+    }
+    return nrOfVisuals;
+}
+
+unsigned int getNrOfCollisions(const iDynTree::Model& model)
+{
+    unsigned int nrOfCollisions = 0;
+    for (LinkIndex index = 0; index < model.getNrOfLinks(); ++index) {
+        nrOfCollisions += model.collisionSolidShapes().linkSolidShapes[index].size();
+    }
+    return nrOfCollisions;
+}
+
+void checkSolidAreEqual(SolidShape* solid, SolidShape* solidCheck)
+{
+    ASSERT_IS_TRUE(solid->isBox() == solidCheck->isBox());
+    ASSERT_IS_TRUE(solid->isCylinder() == solidCheck->isCylinder());
+    ASSERT_IS_TRUE(solid->isSphere() == solidCheck->isSphere());
+    ASSERT_IS_TRUE(solid->isExternalMesh() == solidCheck->isExternalMesh());
+
+    ASSERT_IS_TRUE(solid->name == solidCheck->name);
+    ASSERT_IS_TRUE(solid->nameIsValid == solidCheck->nameIsValid);
+
+    ASSERT_EQUAL_TRANSFORM(solid->link_H_geometry, solidCheck->link_H_geometry);
+
+    if (solid->isBox()) {
+        const Box* box = solid->asBox();
+        const Box* boxCheck = solidCheck->asBox();
+        ASSERT_EQUAL_DOUBLE(box->x, boxCheck->x);
+        ASSERT_EQUAL_DOUBLE(box->y, boxCheck->y);
+        ASSERT_EQUAL_DOUBLE(box->z, boxCheck->z);
+
+    } else if (solid->isCylinder()) {
+        const Cylinder* cylinder = solid->asCylinder();
+        const Cylinder* cylinderCheck = solidCheck->asCylinder();
+        ASSERT_EQUAL_DOUBLE(cylinder->radius, cylinderCheck->radius);
+        ASSERT_EQUAL_DOUBLE(cylinder->length, cylinderCheck->length);
+
+    } else if (solid->isSphere()) {
+        const Sphere* sphere = solid->asSphere();
+        const Sphere* sphereCheck = solidCheck->asSphere();
+        ASSERT_EQUAL_DOUBLE(sphere->radius, sphereCheck->radius);
+
+    } else if (solid->isExternalMesh()) {
+        const ExternalMesh* mesh = solid->asExternalMesh();
+        const ExternalMesh* meshCheck = solidCheck->asExternalMesh();
+        ASSERT_EQUAL_VECTOR(mesh->scale, meshCheck->scale);
+        ASSERT_IS_TRUE(mesh->filename == meshCheck->filename);
+    } else {
+        ASSERT_IS_TRUE(false);
+    }
+
+}
+
 
 void checkImportExportURDF(std::string fileName)
 {
@@ -33,12 +92,6 @@ void checkImportExportURDF(std::string fileName)
     ASSERT_IS_TRUE(ok);
 
     std::cerr << "Model loaded from " << fileName << std::endl;
-    std::ifstream original_urdf(fileName);
-
-    if (original_urdf.is_open()) {
-        std::cout << original_urdf.rdbuf();
-    }
-
     std::string urdfString;
     ModelExporter mdlExporter;
     ok = mdlExporter.init(model, SensorsList());
@@ -58,6 +111,8 @@ void checkImportExportURDF(std::string fileName)
     ASSERT_EQUAL_DOUBLE(model.getNrOfJoints(), modelReloaded.getNrOfJoints());
     ASSERT_EQUAL_DOUBLE(model.getNrOfDOFs(), modelReloaded.getNrOfDOFs());
     ASSERT_EQUAL_DOUBLE(model.getNrOfFrames(), modelReloaded.getNrOfFrames());
+    ASSERT_EQUAL_DOUBLE(getNrOfVisuals(model), getNrOfVisuals(modelReloaded));
+    ASSERT_EQUAL_DOUBLE(getNrOfCollisions(model), getNrOfCollisions(modelReloaded));
 
     // Verify that the link correspond (note that the serialization could have changed)
     for(int lnkIndex=0; lnkIndex < model.getNrOfLinks(); lnkIndex++) {
@@ -68,6 +123,32 @@ void checkImportExportURDF(std::string fileName)
         SpatialInertia inertiaReloaded = modelReloaded.getLink(lnkIndexInReloaded)->getInertia();
         std::cerr << "Testing inertia of link " << model.getLinkName(lnkIndex) << std::endl;
         ASSERT_EQUAL_MATRIX(inertia.asMatrix(), inertiaReloaded.asMatrix());
+
+        // For each link, verify that the visual and collision shape correspond
+
+        // First verify that the number of visual elements are the same
+        ASSERT_EQUAL_DOUBLE(model.visualSolidShapes().linkSolidShapes[lnkIndex].size(),
+                            modelReloaded.visualSolidShapes().linkSolidShapes[lnkIndexInReloaded].size());
+
+        // Then, if there is only one element, verify that it matches
+        if (model.visualSolidShapes().linkSolidShapes[lnkIndex].size() == 1) {
+            SolidShape* solidInModel = model.visualSolidShapes().linkSolidShapes[lnkIndex][0];
+            SolidShape* solidInModelReloaded = modelReloaded.visualSolidShapes().linkSolidShapes[lnkIndexInReloaded][0];
+            std::cerr << "original: " << solidInModel->link_H_geometry.toString() << std::endl
+                      << "reloaded: " << solidInModelReloaded->link_H_geometry.toString() << std::endl;
+            checkSolidAreEqual(solidInModel, solidInModelReloaded);
+        }
+
+        // First verify that the number of visual elements are the same
+        ASSERT_EQUAL_DOUBLE(model.collisionSolidShapes().linkSolidShapes[lnkIndex].size(),
+                            modelReloaded.collisionSolidShapes().linkSolidShapes[lnkIndexInReloaded].size());
+
+        // Then, if there is only one element, verify that it matches
+        if (model.collisionSolidShapes().linkSolidShapes[lnkIndex].size() == 1) {
+            SolidShape* solidInModel = model.collisionSolidShapes().linkSolidShapes[lnkIndex][0];
+            SolidShape* solidInModelReloaded = modelReloaded.collisionSolidShapes().linkSolidShapes[lnkIndexInReloaded][0];
+            checkSolidAreEqual(solidInModel, solidInModelReloaded);
+        }
     }
 
     // Verify that the frame correspond (note that the serialization could have changed)
@@ -94,7 +175,7 @@ int main()
         }
 
         std::string urdfFileName = getAbsModelPath(std::string(IDYNTREE_TESTS_URDFS[mdl]));
-        std::cout << "Checking URDF import/export test on " << urdfFileName << std::endl;
+        std::cout << "Checking model import/export test on " << urdfFileName << std::endl;
         checkImportExportURDF(urdfFileName);
     }
 

From 4aa9ca150736a22adc85fb0231a0469ab0cc5119 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 26 Aug 2019 15:35:49 +0200
Subject: [PATCH 142/194] Bump version to 0.11.104

To permit to downstream dependencies to depend on the URDF exporter added in devel.
---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 988a7d6514e..e21d1f53750 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,7 +25,7 @@ set(VARS_PREFIX "iDynTree")
 
 set(${VARS_PREFIX}_MAJOR_VERSION 0)
 set(${VARS_PREFIX}_MINOR_VERSION 11)
-set(${VARS_PREFIX}_PATCH_VERSION 103)
+set(${VARS_PREFIX}_PATCH_VERSION 104)
 set(${VARS_PREFIX}_VERSION ${${VARS_PREFIX}_MAJOR_VERSION}.${${VARS_PREFIX}_MINOR_VERSION}.${${VARS_PREFIX}_PATCH_VERSION})
 
 # Pick up our CMake scripts - they are all in the cmake subdirectory.

From 80d7452c134118c64b3193d5e0d91a207230c6dc Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 12 Sep 2019 13:21:21 +0200
Subject: [PATCH 143/194] Add ModelCalibrationHelper class

---
 doc/releases/v0_12.md                         |   1 +
 src/model_io/urdf/CMakeLists.txt              |   2 +
 .../iDynTree/ModelIO/ModelCalibrationHelper.h | 120 ++++++++++++++++++
 .../urdf/src/ModelCalibrationHelper.cpp       | 111 ++++++++++++++++
 src/model_io/urdf/tests/CMakeLists.txt        |   1 +
 .../tests/ModelCalibrationHelperUnitTest.cpp  |  81 ++++++++++++
 6 files changed, 316 insertions(+)
 create mode 100644 src/model_io/urdf/include/iDynTree/ModelIO/ModelCalibrationHelper.h
 create mode 100644 src/model_io/urdf/src/ModelCalibrationHelper.cpp
 create mode 100644 src/model_io/urdf/tests/ModelCalibrationHelperUnitTest.cpp

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index bfb9d223e24..6d41ef424b4 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -64,6 +64,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 ### `modelio`
 * Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
 * Added support in the URDF parser to correctly parse the optional name parameter of visual and collision elements.
+* Added `iDynTree::ModelCalibrationHelper` to simplify loading a model from file, update its inertial parameters and exporting again to file (https://github.com/robotology/idyntree/pull/576).
 
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
diff --git a/src/model_io/urdf/CMakeLists.txt b/src/model_io/urdf/CMakeLists.txt
index 4bf11e1515c..8c45251f385 100644
--- a/src/model_io/urdf/CMakeLists.txt
+++ b/src/model_io/urdf/CMakeLists.txt
@@ -15,6 +15,7 @@ endif()
 set(IDYNTREE_MODELIO_URDF_HEADERS include/iDynTree/ModelIO/URDFDofsImport.h
                                   include/iDynTree/ModelIO/ModelLoader.h
                                   include/iDynTree/ModelIO/ModelExporter.h
+                                  include/iDynTree/ModelIO/ModelCalibrationHelper.h
                                   include/deprecated/iDynTree/ModelIO/URDFModelImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFGenericSensorsImport.h
                                   include/deprecated/iDynTree/ModelIO/URDFSolidShapesImport.h)
@@ -48,6 +49,7 @@ set(IDYNTREE_MODELIO_URDF_XMLELEMENTS_SOURCES src/URDFDocument.cpp
 set(IDYNTREE_MODELIO_URDF_SOURCES src/URDFDofsImport.cpp
                                   src/ModelLoader.cpp
                                   src/ModelExporter.cpp
+                                  src/ModelCalibrationHelper.cpp
                                   src/deprecated/URDFModelImport.cpp
                                   src/deprecated/URDFGenericSensorsImport.cpp
                                   src/deprecated/URDFSolidShapesImport.cpp
diff --git a/src/model_io/urdf/include/iDynTree/ModelIO/ModelCalibrationHelper.h b/src/model_io/urdf/include/iDynTree/ModelIO/ModelCalibrationHelper.h
new file mode 100644
index 00000000000..a88c5f1caab
--- /dev/null
+++ b/src/model_io/urdf/include/iDynTree/ModelIO/ModelCalibrationHelper.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef IDYNTREE_MODEL_CALIBRATION_HELPER_H
+#define IDYNTREE_MODEL_CALIBRATION_HELPER_H
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/Sensors/Sensors.h>
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+
+
+/**
+ * \ingroup iDynTreeModelIO
+ *
+ * Helper class to load a model, modify its parameters based on calibration,
+ * and save it again to file.
+ *
+ */
+class ModelCalibrationHelper
+{
+private:
+    class ModelCalibrationHelperPimpl;
+    std::unique_ptr<ModelCalibrationHelperPimpl> m_pimpl;
+
+public:
+
+    /**
+     * @name Constructor/Destructor
+     */
+    //@{
+
+    /**
+     * Constructor
+     *
+     */
+    ModelCalibrationHelper();
+
+    ~ModelCalibrationHelper();
+
+    //@}
+
+    /**
+     * Load the model of the robot  from a string.
+     *
+     * @param modelString string containg the model of the robot.
+     * @param filetype type of the file to load, currently supporting only urdf type.
+     *
+     */
+    bool loadModelFromString(const std::string & modelString, const std::string & filetype="urdf");
+
+    /**
+     * Load the model of the robot from an external file.
+     *
+     * @param filename path to the file to load
+     * @param filetype type of the file to load, currently supporting only urdf type.
+     *
+     */
+    bool loadModelFromFile(const std::string & filename, const std::string & filetype="urdf");
+
+    /**
+     * Update the inertial parameters of the loaded model.
+     *
+     * @note the inertial params vector follow the structure of the Model::getInertialParameters method.
+     */
+    bool updateModelInertialParametersToString(std::string & modelString, 
+                                               const iDynTree::VectorDynSize& inertialParams,
+                                               const std::string filetype="urdf",
+                                               const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions());
+
+    /**
+     * Update the inertial parameters of the loaded model.
+     *
+     * @note the inertial params vector follows the structure of the Model::getInertialParameters method.
+     */
+    bool updateModelInertialParametersToFile(const std::string & filename, 
+                                             const iDynTree::VectorDynSize& inertialParams,
+                                             const std::string filetype="urdf",
+                                             const iDynTree::ModelExporterOptions options=iDynTree::ModelExporterOptions());
+
+    /**
+     * Get the loaded model.
+     *
+     * @note This always return the model loaded via loadModel methods, and is not affected by the updateModel methods.
+     */
+    const Model & model();
+
+    /**
+     * Get the loaded sensors.
+     * 
+     * @note This always return the model loaded via loadModel method, and is not affected by the updateModel methods.
+     */
+    const SensorsList & sensors();
+
+    /**
+     * Return true if the model have been correctly true.
+     *
+     * @note This always return the validity of the model loaded via loadModel method, and is not affected by the updateModel methods.
+     * @return True if the model was loaded correctly.
+     */
+    bool isValid();
+    //@}
+};
+
+}
+
+#endif
diff --git a/src/model_io/urdf/src/ModelCalibrationHelper.cpp b/src/model_io/urdf/src/ModelCalibrationHelper.cpp
new file mode 100644
index 00000000000..f0ac0843028
--- /dev/null
+++ b/src/model_io/urdf/src/ModelCalibrationHelper.cpp
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/ModelIO/ModelCalibrationHelper.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/ModelIO/ModelExporter.h>
+
+#include <string>
+#include <vector>
+
+namespace iDynTree
+{
+    class ModelCalibrationHelper::ModelCalibrationHelperPimpl {
+    public:
+        ModelLoader modelLoader;
+        ModelExporter modelExporter;
+
+        ModelCalibrationHelperPimpl() {}
+    };
+
+    ModelCalibrationHelper::ModelCalibrationHelper()
+    : m_pimpl(new ModelCalibrationHelperPimpl())
+    {
+    }
+
+    ModelCalibrationHelper::~ModelCalibrationHelper() {}
+
+    const Model& ModelCalibrationHelper::model()
+    {
+        return m_pimpl->modelLoader.model();
+    }
+
+    const SensorsList& ModelCalibrationHelper::sensors()
+    {
+        return m_pimpl->modelLoader.sensors();
+    }
+
+    bool ModelCalibrationHelper::isValid()
+    {
+        return m_pimpl->modelLoader.isValid();
+    }
+
+    bool ModelCalibrationHelper::loadModelFromString(const std::string& xmlString,
+                                                     const std::string& filetype)
+    {
+        return m_pimpl->modelLoader.loadModelFromString(xmlString, filetype);
+    }
+
+    bool ModelCalibrationHelper::loadModelFromFile(const std::string& filename,
+                                        const std::string& filetype)
+    {
+        return m_pimpl->modelLoader.loadModelFromFile(filename, filetype);
+    }
+
+    bool ModelCalibrationHelper::updateModelInertialParametersToString(std::string & model_string, 
+                                                                       const iDynTree::VectorDynSize& inertialParams,
+                                                                       const std::string filetype,
+                                                                       const ModelExporterOptions options)
+    {
+        Model exportedModel = this->model();
+        SensorsList exportedSensors = this->sensors();
+        
+        bool ok = exportedModel.updateInertialParameters(inertialParams);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToString", "Error in iDynTree::Model::updateInertialParameters method.");
+            return false;
+        }
+        
+        ok = m_pimpl->modelExporter.init(exportedModel, exportedSensors, options);
+        ok = ok && m_pimpl->modelExporter.exportModelToString(model_string, filetype);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToString", "Error in ModelExporter::exportModelToString method.");
+            return false;
+        }
+
+        return true;
+    }
+
+
+    bool ModelCalibrationHelper::updateModelInertialParametersToFile(const std::string & filename, 
+                                                                     const iDynTree::VectorDynSize& inertialParams,
+                                                                     const std::string filetype,
+                                                                     const ModelExporterOptions options)
+    {
+        Model exportedModel = this->model();
+        SensorsList exportedSensors = this->sensors();
+
+        bool ok = exportedModel.updateInertialParameters(inertialParams);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToFile", "Error in iDynTree::Model::updateInertialParameters method.");
+            return false;
+        }
+
+        ok = m_pimpl->modelExporter.init(exportedModel, exportedSensors, options);
+        ok = ok && m_pimpl->modelExporter.exportModelToFile(filename, filetype);
+        if (!ok) {
+            reportError("ModelCalibrationHelper", "updateModelInertialParametersToFile", "Error in ModelExporter::exportModelToFile method.");
+            return false;
+        }
+        
+        return true;
+    }
+
+}
diff --git a/src/model_io/urdf/tests/CMakeLists.txt b/src/model_io/urdf/tests/CMakeLists.txt
index 939527fd55e..8698f41d56a 100644
--- a/src/model_io/urdf/tests/CMakeLists.txt
+++ b/src/model_io/urdf/tests/CMakeLists.txt
@@ -24,6 +24,7 @@ macro(add_modelio_urdf_unit_test classname)
 endmacro()
 
 add_modelio_urdf_unit_test(URDFModelImport)
+add_modelio_urdf_unit_test(ModelCalibrationHelper)
 add_modelio_urdf_unit_test(ModelExporter)
 add_modelio_urdf_unit_test(URDFGenericSensorImport)
 add_modelio_urdf_unit_test(PredictSensorsMeasurement)
diff --git a/src/model_io/urdf/tests/ModelCalibrationHelperUnitTest.cpp b/src/model_io/urdf/tests/ModelCalibrationHelperUnitTest.cpp
new file mode 100644
index 00000000000..a3b13c6b68f
--- /dev/null
+++ b/src/model_io/urdf/tests/ModelCalibrationHelperUnitTest.cpp
@@ -0,0 +1,81 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include "testModels.h"
+
+#include <iDynTree/Core/TestUtils.h>
+
+#include <iDynTree/Model/Model.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+#include <iDynTree/ModelIO/ModelCalibrationHelper.h>
+
+#include <cassert>
+#include <cstdio>
+#include <cstdlib>
+#include <algorithm>
+
+using namespace iDynTree;
+
+double getTotalMass(const Model& model)
+{
+    double totalMass = 0.0;
+    for(size_t l=0; l < model.getNrOfLinks(); l++)
+    {
+        totalMass += model.getLink(l)->getInertia().getMass();
+    }
+
+    return totalMass;
+}
+
+int main()
+{
+    // Open file with a single link with mass 1
+    std::string urdfFileName = getAbsModelPath("oneLink.urdf");
+
+    ModelCalibrationHelper mdlCalibHelper;
+    bool ok = mdlCalibHelper.loadModelFromFile(urdfFileName);
+    ASSERT_IS_TRUE(ok);
+
+    // Check that the mass is one
+    double expectedMass = 1.0;
+    ASSERT_EQUAL_DOUBLE(getTotalMass(mdlCalibHelper.model()), expectedMass);
+
+    // Modify the mass as an inertial paramters
+    VectorDynSize inertialParams(10*mdlCalibHelper.model().getNrOfLinks());
+
+    mdlCalibHelper.model().getInertialParameters(inertialParams);
+
+    ASSERT_EQUAL_DOUBLE(inertialParams(0), expectedMass);
+    double newMass = 2.0;
+    inertialParams(0) = newMass;
+
+    std::string newModel;
+    ok = mdlCalibHelper.updateModelInertialParametersToString(newModel, inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    // Write to file for debug
+    ok = mdlCalibHelper.updateModelInertialParametersToFile("ModelCalibrationHelperUnitTestModel.urdf", inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    std::cerr << newModel << std::endl;
+
+    // Verify mass
+    ModelLoader mdlLoader;
+
+    ok = mdlLoader.loadModelFromString(newModel);
+    ASSERT_IS_TRUE(ok);
+    ASSERT_EQUAL_DOUBLE(getTotalMass(mdlLoader.model()), newMass);
+
+
+
+
+
+    return EXIT_SUCCESS;
+}

From fb68adf8d730969a072bd32f9f175cedc22fc397 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Fri, 13 Sep 2019 09:43:54 +0200
Subject: [PATCH 144/194] Ensure that any model loaded as a "normalized" joint
 ordering, at least w.r.t. its default base (#492)

---
 doc/releases/v0_12.md                         |  1 +
 .../iDynTree/Model/ModelTransformers.h        | 21 ++++++++
 src/model/src/ModelTransformers.cpp           | 48 +++++++++++++++++++
 .../include/iDynTree/ModelIO/ModelLoader.h    |  4 ++
 src/model_io/urdf/src/URDFDocument.cpp        | 18 +++++--
 5 files changed, 87 insertions(+), 5 deletions(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 6d41ef424b4..fbb2d60d1ce 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -65,6 +65,7 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 * Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
 * Added support in the URDF parser to correctly parse the optional name parameter of visual and collision elements.
 * Added `iDynTree::ModelCalibrationHelper` to simplify loading a model from file, update its inertial parameters and exporting again to file (https://github.com/robotology/idyntree/pull/576).
+* Updated `iDynTree::ModelLoader` class to load by default models with normalized joint ordering (https://github.com/robotology/idyntree/issues/491).
 
 ### `yarprobotstatepublisher`
 * Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
diff --git a/src/model/include/iDynTree/Model/ModelTransformers.h b/src/model/include/iDynTree/Model/ModelTransformers.h
index 4ffe3fdebcb..d256d8f0a93 100644
--- a/src/model/include/iDynTree/Model/ModelTransformers.h
+++ b/src/model/include/iDynTree/Model/ModelTransformers.h
@@ -66,6 +66,27 @@ bool createReducedModel(const Model& fullModel,
                         const std::vector<std::string>& jointsInReducedModel,
                         Model& reducedModel);
 
+/**
+ * @brief Given a specified base, return a model with a "normalized" joint numbering for that base.
+ *
+ * This function takes in input a iDynTree::Model and a name of a link in that model.
+ * It returns a model identical to the one in input, but with the joint serialization
+ * of the non-fixed joint modified in such a way that a non-fixed joint has an index higher than
+ * all the non-fixed joints on the path between it and the base. After all the non-fixed joints,
+ * the fixed joints are also added with the same criteria, but applied to fixed joints.
+ *
+ * @note This method make sure that the non-fixed joint in the model have a "regular numbering"
+ *       as described in Featherstone "Rigid Body Dynamics Algorithm", section 4.1.2 . Note that
+ *       this numbering is not required by any algorithm in iDynTree, but it may be useful for
+ *       example to ensure that for a chain model the joint numbering is the one induced by the
+ *       kinematic structure.
+ *
+ * @return true if all went well, false if there was an error in conversion.
+ */
+bool createModelWithNormalizedJointNumbering(const Model& model,
+                                             const std::string& baseForNormalizedJointNumbering,
+                                             Model& reducedModel);
+
 
 }
 
diff --git a/src/model/src/ModelTransformers.cpp b/src/model/src/ModelTransformers.cpp
index dd6d7864893..28da0ec9682 100644
--- a/src/model/src/ModelTransformers.cpp
+++ b/src/model/src/ModelTransformers.cpp
@@ -504,5 +504,53 @@ bool createReducedModel(const Model& fullModel,
     return ok;
 }
 
+bool createModelWithNormalizedJointNumbering(const Model& model,
+                                             const std::string& baseForNormalizedJointNumbering,
+                                             Model& normalizedModel)
+{
+    if (!model.isLinkNameUsed(baseForNormalizedJointNumbering))
+    {
+        std::cerr << "[ERROR] createModelWithNormalizedJointNumbering error : "
+                      << " Link " << baseForNormalizedJointNumbering << " not found in the input model"
+                      << std::endl;
+        return false;
+    }
+
+    Traversal traversal;
+    model.computeFullTreeTraversal(traversal, model.getLinkIndex(baseForNormalizedJointNumbering));
+
+    // Ordering for non-fixed joints
+    std::vector<std::string> jointOrderingNonFixed;
+
+    // Ordering for fixed joints
+    std::vector<std::string> jointOrderingFixed;
+
+    // Iterate with the traversal to compute the normalized order
+    for(TraversalIndex traversalEl=1; traversalEl < traversal.getNrOfVisitedLinks(); traversalEl++)
+    {
+        IJointConstPtr visitedJoint = traversal.getParentJoint(traversalEl);
+        if (visitedJoint->getNrOfDOFs() !=0)
+        {
+            jointOrderingNonFixed.push_back(model.getJointName(visitedJoint->getIndex()));
+        }
+        else
+        {
+            jointOrderingFixed.push_back(model.getJointName(visitedJoint->getIndex()));
+        }
+    }
+
+    // Compute complete ordering
+    std::vector<std::string> jointOrdering;
+    jointOrdering.insert(jointOrdering.end(), jointOrderingNonFixed.begin(), jointOrderingNonFixed.end());
+    jointOrdering.insert(jointOrdering.end(), jointOrderingFixed.begin(), jointOrderingFixed.end());
+
+    assert(jointOrdering.size() == model.getNrOfJoints());
+
+    // Create the new model
+    bool ok = createReducedModel(model, jointOrdering, normalizedModel);
+
+    return ok;
+}
+
 
 }
diff --git a/src/model_io/urdf/include/iDynTree/ModelIO/ModelLoader.h b/src/model_io/urdf/include/iDynTree/ModelIO/ModelLoader.h
index fd8909662ba..401517e84dd 100644
--- a/src/model_io/urdf/include/iDynTree/ModelIO/ModelLoader.h
+++ b/src/model_io/urdf/include/iDynTree/ModelIO/ModelLoader.h
@@ -65,6 +65,10 @@ struct ModelParserOptions
  *
  * Helper class to load a model from a generic format.
  *
+ * Unless the methods for loading a model with an explicit serialization are used,
+ * the default joint serialization of the model loaded will be a "normalized" joint serialization
+ * based on the default base link, see the iDynTree::createModelWithNormalizedJointNumbering function
+ * for more details.
  */
 class ModelLoader
 {
diff --git a/src/model_io/urdf/src/URDFDocument.cpp b/src/model_io/urdf/src/URDFDocument.cpp
index adf60730dad..3690b8ec219 100644
--- a/src/model_io/urdf/src/URDFDocument.cpp
+++ b/src/model_io/urdf/src/URDFDocument.cpp
@@ -15,6 +15,7 @@
 
 #include <iDynTree/Model/PrismaticJoint.h>
 #include <iDynTree/Model/RevoluteJoint.h>
+#include <iDynTree/Model/ModelTransformers.h>
 #include <iDynTree/Sensors/SixAxisForceTorqueSensor.h>
 
 #include <algorithm>
@@ -27,7 +28,7 @@ namespace iDynTree {
                                                          std::unordered_map<std::string, JointElement::JointInfo>& joints,
                                                          std::unordered_map<std::string, JointElement::JointInfo>& fixed_joints);
     static bool isFakeLink(const iDynTree::Model& modelWithFakeLinks, const iDynTree::LinkIndex linkToCheck);
-    static bool removeFakeLinks(const iDynTree::Model &originalModel, iDynTree::Model& cleanModel);
+    static bool p_removeFakeLinks(const iDynTree::Model &originalModel, iDynTree::Model& cleanModel);
     static bool processSensors(const Model& model,
                                const std::vector<std::shared_ptr<SensorHelper>>& helpers,
                                iDynTree::SensorsList& sensors);
@@ -119,13 +120,20 @@ namespace iDynTree {
 
         // set the default root in the model
         m_model.setDefaultBaseLink(m_model.getLinkIndex(rootCandidates[0]));
-        iDynTree::Model newModel;
+        iDynTree::Model newModel, normalizedModel;
 
-        if (!removeFakeLinks(m_model, newModel)) {
+        if (!p_removeFakeLinks(m_model, newModel)) {
             reportError("URDFDocument", "documentHasBeenParsed", "Failed to remove fake links from the model");
             return false;
         }
-        m_model = newModel;
+
+        std::string baseLinkName = newModel.getLinkName(newModel.getDefaultBaseLink());
+        if (!createModelWithNormalizedJointNumbering(newModel, baseLinkName, normalizedModel)) {
+            reportError("URDFDocument", "documentHasBeenParsed", "Failed to remove fake links from the model");
+            return false;
+        }
+
+        m_model = normalizedModel;
 
         if (!processSensors(m_model, m_buffers.sensorHelpers, m_sensors))
         {
@@ -237,7 +245,7 @@ namespace iDynTree {
         return true;
     }
 
-    bool removeFakeLinks(const iDynTree::Model &originalModel, iDynTree::Model& cleanModel)
+    bool p_removeFakeLinks(const iDynTree::Model &originalModel, iDynTree::Model& cleanModel)
     {
         // Clear the output model
         cleanModel = iDynTree::Model();

From f8b1b891aa365e810b305df518e69d52999b9e31 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 17 Sep 2019 22:14:30 +0200
Subject: [PATCH 145/194] TestUtils: fail also when nan is part of the
 comparison

Given how they were written, until now the checks contained in TestUtils were
successful even if one of the terms was actually a NaN . This commit changes the
check to fail if one or two NaN are compared.
---
 src/core/include/iDynTree/Core/TestUtils.h | 2 +-
 src/core/src/TestUtils.cpp                 | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/core/include/iDynTree/Core/TestUtils.h b/src/core/include/iDynTree/Core/TestUtils.h
index 672fd2791e8..5bc85da2e85 100644
--- a/src/core/include/iDynTree/Core/TestUtils.h
+++ b/src/core/include/iDynTree/Core/TestUtils.h
@@ -297,7 +297,7 @@ namespace iDynTree
 
         for( unsigned int i = 0; i < vec1.size(); i++ )
         {
-            if( fabs(vec1(i)-vec2(i)) >= tol )
+            if( !(fabs(vec1(i)-vec2(i)) < tol) )
             {
                 checkCorrect = false;
                 correctElements[i] = false;
diff --git a/src/core/src/TestUtils.cpp b/src/core/src/TestUtils.cpp
index f6ff906e90e..033224b996b 100644
--- a/src/core/src/TestUtils.cpp
+++ b/src/core/src/TestUtils.cpp
@@ -47,7 +47,7 @@ void assertTrue(bool prop, std::string file, int line)
 
 void assertDoubleAreEqual(const double& val1, const double& val2, double tol, std::string file, int line)
 {
-    if( fabs(val1-val2) >= tol )
+    if( !(fabs(val1-val2) < tol) )
     {
        std::cerr << file << ":" << line << " : assertDoubleAreEqual failure: val1 is " << val1
                   << " while val2 is " << val2 << std::endl;

From e12aa44bcbbb3162321fde71d44a9d0620c3bbf6 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 17 Sep 2019 22:16:39 +0200
Subject: [PATCH 146/194] Add SolidShapes library that depends on Assimp and
 related inertia parameter estimation helpers

This commit adds a SolidShape helper library. This library is part of iDynTree, and is meant
to contain all the algorithms that use in some form the visual and collision geometries of the model,
and so they depend on the Assimp library to load meshes.

The first algorithm added to the new library is an helper function that provides rough estimates
of the inertial parameters (mass, first moments of mass, 3d inertia matrix elements) of a robot
given the total mass of the robot, and its collisions shapes. While the estimates provided are
quite rough, they can be quite useful at least to provide an expected order of magnitute of the parameters,
to normalize errors or as initial points of a nonlinear optimization procedure.
---
 README.md                                     |   1 +
 cmake/iDynTreeDependencies.cmake              |   1 +
 src/CMakeLists.txt                            |   1 +
 .../include/iDynTree/Model/SolidShapes.h      |   8 +
 .../urdf/tests/URDFModelImportUnitTest.cpp    |   2 +-
 src/solid-shapes/CMakeLists.txt               |  39 ++
 .../InertialParametersSolidShapesHelpers.h    |  39 ++
 .../InertialParametersSolidShapesHelpers.cpp  | 392 ++++++++++++++++++
 src/solid-shapes/tests/CMakeLists.txt         |  24 ++
 ...etersSolidShapesHelpersIntegrationTest.cpp | 111 +++++
 src/tests/data/cube.stl                       |  86 ++++
 src/tests/data/oneLink.urdf                   |  12 +
 src/tests/integration/CMakeLists.txt          |  11 +-
 ...etersSolidShapesHelpersIntegrationTest.cpp |  78 ++++
 14 files changed, 801 insertions(+), 4 deletions(-)
 create mode 100644 src/solid-shapes/CMakeLists.txt
 create mode 100644 src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h
 create mode 100644 src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp
 create mode 100644 src/solid-shapes/tests/CMakeLists.txt
 create mode 100644 src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp
 create mode 100644 src/tests/data/cube.stl
 create mode 100644 src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp

diff --git a/README.md b/README.md
index 8c8a0082538..38774d7c4fe 100644
--- a/README.md
+++ b/README.md
@@ -35,6 +35,7 @@ but you can manually make sure that iDynTree searches or ignores a given depende
 - [Libxml2](http://xmlsoft.org/)
 
 ##### Optional
+- [Assimp](http://www.assimp.org/)
 - [IPOPT](https://projects.coin-or.org/Ipopt)
 - [Qt5](https://www.qt.io/)
 - [YARP](https://github.com/robotology/yarp)
diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 0e35ef70fd5..e8d46eb89ec 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -57,3 +57,4 @@ idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
 idyntree_handle_dependency(ALGLIB 3.14.0)
 idyntree_handle_dependency(WORHP)
+idyntree_handle_dependency(ASSIMP)
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 063687913d3..f63c2654397 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -18,6 +18,7 @@ add_subdirectory(model)
 add_subdirectory(sensors)
 add_subdirectory(model_io)
 add_subdirectory(estimation)
+add_subdirectory(solid-shapes)
 add_subdirectory(high-level)
 
 if (IDYNTREE_USES_IPOPT)
diff --git a/src/model/include/iDynTree/Model/SolidShapes.h b/src/model/include/iDynTree/Model/SolidShapes.h
index 0560e484ef3..0a4e75fedd6 100644
--- a/src/model/include/iDynTree/Model/SolidShapes.h
+++ b/src/model/include/iDynTree/Model/SolidShapes.h
@@ -67,6 +67,14 @@ namespace iDynTree
         double radius;
     };
 
+    /**
+     * @brief Box, i.e. 3D rectangular parallelepiped.
+     *
+     * The box is centered in the mesh frame, its sides
+     * are aligned with the axis of the mesh frame, and
+     * the side lenghts in the x, y and z direction are given
+     * by the attributes x, y and z.
+     */
     class Box: public SolidShape
     {
     public:
diff --git a/src/model_io/urdf/tests/URDFModelImportUnitTest.cpp b/src/model_io/urdf/tests/URDFModelImportUnitTest.cpp
index 3d923ac9ba9..bd15fef12e5 100644
--- a/src/model_io/urdf/tests/URDFModelImportUnitTest.cpp
+++ b/src/model_io/urdf/tests/URDFModelImportUnitTest.cpp
@@ -225,7 +225,7 @@ void checkLoadReducedModelOrderIsKept(std::string urdfFileName)
 int main()
 {
     checkURDF(getAbsModelPath("/simple_model.urdf"),1,0,0,1, 1, 0, "link1");
-    checkURDF(getAbsModelPath("/oneLink.urdf"),1,0,0,7,0,0,"link1");
+    checkURDF(getAbsModelPath("/oneLink.urdf"),1,0,0,7,1,1,"link1");
     checkURDF(getAbsModelPath("twoLinks.urdf"),2,1,1,6,0,0,"link1");
     checkURDF(getAbsModelPath("icub_skin_frames.urdf"),39,38,32,62, 28, 28,"root_link");
     checkURDF(getAbsModelPath("iCubGenova02.urdf"),33,32,26,111, 33, 33, "root_link");
diff --git a/src/solid-shapes/CMakeLists.txt b/src/solid-shapes/CMakeLists.txt
new file mode 100644
index 00000000000..36a5d19c576
--- /dev/null
+++ b/src/solid-shapes/CMakeLists.txt
@@ -0,0 +1,39 @@
+set(libraryname idyntree-solid-shapes)
+
+set(IDYNTREE_SOLID_SHAPES_SOURCES src/InertialParametersSolidShapesHelpers.cpp)
+set(IDYNTREE_SOLID_SHAPES_HEADERS include/iDynTree/InertialParametersSolidShapesHelpers.h)
+
+add_library(${libraryname} ${IDYNTREE_SOLID_SHAPES_HEADERS} ${IDYNTREE_SOLID_SHAPES_SOURCES})
+
+target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+                                                 "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
+
+
+target_link_libraries(${libraryname} PUBLIC idyntree-core idyntree-model)
+target_include_directories(${libraryname} PRIVATE ${EIGEN3_INCLUDE_DIR})
+
+
+if (IDYNTREE_USES_ASSIMP)
+  target_compile_definitions(${libraryname} PRIVATE IDYNTREE_USES_ASSIMP)
+  target_include_directories(${libraryname} PRIVATE "${ASSIMP_INCLUDE_DIRS}")
+  link_directories("${ASSIMP_LIBRARY_DIRS}")
+  target_link_libraries(${libraryname} PRIVATE "${ASSIMP_LIBRARIES}")
+endif()
+
+set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_SOLID_SHAPES_HEADERS})
+
+install(TARGETS ${libraryname}
+        EXPORT iDynTree
+        COMPONENT runtime
+        RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin
+        LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib
+        ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib
+        PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree)
+
+set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
+set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
+
+
+if(IDYNTREE_COMPILE_TESTS)
+    add_subdirectory(tests)
+endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h b/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h
new file mode 100644
index 00000000000..bc1a6446436
--- /dev/null
+++ b/src/solid-shapes/include/iDynTree/InertialParametersSolidShapesHelpers.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#ifndef IDYNTREE_INERTIAL_PARAMETERS_HELPERS_H
+#define IDYNTREE_INERTIAL_PARAMETERS_HELPERS_H
+
+#include <iDynTree/Model/Model.h>
+
+namespace iDynTree
+{
+
+/**
+ * @brief Estimate the inertial parameters of a model using link bounding boxes and the total mass.
+ *
+ * @param[in] totalMass The total mass of the model, in Kilograms.
+ * @param[in] model The model, used to extract the bounding box of the links.
+ * @param[out] inertialParameters vector of inertial parameters, in the format used by Model::getInertialParameters
+ * @return true if all went well, false otherwise .
+ *
+ * @note if inertialParameters does not have the correct size, it will be resized.
+ * @warning This function needs to allocate some internal buffers, and so it performs dynamic memory allocation.
+ *
+ * @note This function requires IDYNTREE_USES_ASSIMP to be true, otherwise it always returns false.
+ */
+bool estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(const double totalMass,
+                                                                 iDynTree::Model& model,
+                                                                 VectorDynSize& estimatedInertialParams);
+
+
+}
+
+#endif /* IDYNTREE_INERTIAL_PARAMETERS_HELPERS_H */
diff --git a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp
new file mode 100644
index 00000000000..6c7355e466e
--- /dev/null
+++ b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp
@@ -0,0 +1,392 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/InertialParametersSolidShapesHelpers.h>
+
+#include <iDynTree/Core/EigenHelpers.h>
+#include <iDynTree/Core/SpatialInertia.h>
+#include <iDynTree/Core/Utils.h>
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Core/VectorFixSize.h>
+
+#include <iDynTree/Model/SolidShapes.h>
+
+#ifdef IDYNTREE_USES_ASSIMP
+#include <assimp/Importer.hpp>
+#include <assimp/scene.h>
+#endif
+
+namespace iDynTree
+{
+
+// Helper methods for the box shape
+// This should be eventually be moved to be methods of the Box class
+SpatialInertia boxGet6DInertiaInLinkFrameFromDensity(const Box& box,
+                                                     double density)
+{
+    double boxVolume = box.x*box.y*box.z;
+    double boxMass   = density*boxVolume;
+    // Assuming uniform density, the center of mass is coincident with the box center
+    PositionRaw comInGeomFrame;
+    comInGeomFrame.zero();
+    // From http://scienceworld.wolfram.com/physics/MomentofInertiaRectangularParallelepiped.html
+    RotationalInertiaRaw rotInertiaInGeomFrame;
+    rotInertiaInGeomFrame.zero();
+    double x2 = box.x*box.x;
+    double y2 = box.y*box.y;
+    double z2 = box.z*box.z;
+    rotInertiaInGeomFrame(0, 0) = (boxMass/12.0)*(y2+z2);
+    rotInertiaInGeomFrame(1, 1) = (boxMass/12.0)*(x2+z2);
+    rotInertiaInGeomFrame(2, 2) = (boxMass/12.0)*(x2+y2);
+
+    SpatialInertia inertiaInGeometryFrame = SpatialInertia(boxMass, comInGeomFrame, rotInertiaInGeomFrame);
+
+    return box.link_H_geometry*inertiaInGeometryFrame;
+}
+
+double boxGetVolume(const Box& box)
+{
+    return box.x*box.y*box.z;
+}
+
+#ifdef IDYNTREE_USES_ASSIMP
+
+// Inspired  from https://github.com/ros-visualization/rviz/blob/070835c426b8982e304b38eb4a9c6eb221155d5f/src/rviz/mesh_loader.cpp#L644
+void buildMesh(const aiScene* scene, const aiNode* node, const double scale, std::vector<aiVector3D>& vertexVector)
+{
+  if (!node)
+  {
+    return;
+  }
+
+  aiMatrix4x4 transform = node->mTransformation;
+  aiNode *pnode = node->mParent;
+  while (pnode)
+  {
+    // Don't convert to y-up orientation, which is what the root node in
+    // Assimp does
+    if (pnode->mParent != NULL)
+      transform = pnode->mTransformation * transform;
+    pnode = pnode->mParent;
+  }
+
+  aiMatrix3x3 rotation(transform);
+  aiMatrix3x3 inverse_transpose_rotation(rotation);
+  inverse_transpose_rotation.Inverse();
+  inverse_transpose_rotation.Transpose();
+
+  for (uint32_t i = 0; i < node->mNumMeshes; i++)
+  {
+    aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
+
+    // Add the vertices
+    for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
+    {
+      aiVector3D p = input_mesh->mVertices[j];
+      p *= transform;
+      p *= scale;
+      vertexVector.push_back(p);
+    }
+  }
+
+  for (uint32_t i=0; i < node->mNumChildren; ++i)
+  {
+    buildMesh(scene, node->mChildren[i], scale, vertexVector);
+  }
+}
+
+/**
+ * Convert std::vector<aiVector3D> to std::vector<iDynTree::Position>
+ */
+std::vector<Position> toiDynTree(std::vector<aiVector3D> assimpPoints)
+{
+    std::vector<Position> ret;
+
+    for (auto&& assimpPoint : assimpPoints) {
+        ret.push_back(Position(assimpPoint.x, assimpPoint.y, assimpPoint.z));
+    }
+
+    return ret;
+}
+
+Box extractAABBFromVertices(const Transform& link_H_vertices,
+                        const std::vector<Position>& vertexVector)
+{
+    Box box;
+
+    double minX = vertexVector[0](0);
+    double maxX = vertexVector[0](0);
+    double minY = vertexVector[0](1);
+    double maxY = vertexVector[0](1);
+    double minZ = vertexVector[0](2);
+    double maxZ = vertexVector[0](2);
+
+    for(size_t i=0; i < vertexVector.size(); i++)
+    {
+        if (vertexVector[i](0) > maxX)
+        {
+            maxX = vertexVector[i](0);
+        }
+
+        if (vertexVector[i](0) < minX)
+        {
+            minX = vertexVector[i](0);
+        }
+
+        if (vertexVector[i](1) > maxY)
+        {
+            maxY = vertexVector[i](1);
+        }
+
+        if (vertexVector[i](1) < minY)
+        {
+            minY = vertexVector[i](1);
+        }
+
+        if (vertexVector[i](2) > maxZ)
+        {
+            maxZ = vertexVector[i](2);
+        }
+
+        if (vertexVector[i](2) < minZ)
+        {
+            minZ = vertexVector[i](2);
+        }
+    }
+
+    // The side of the BB is the difference between the max and min
+    box.x = maxX-minX;
+    box.y = maxY-minY;
+    box.z = maxZ-minZ;
+
+    // The offset between the geometry origin and the bounding box origin is the middle point
+    Position offset_bb_wrt_geom;
+    offset_bb_wrt_geom(0) = (maxX+minX)/2.0;
+    offset_bb_wrt_geom(1) = (maxY+minY)/2.0;
+    offset_bb_wrt_geom(2) = (maxZ+minZ)/2.0;
+
+    Position offset_bb_wrt_link = link_H_vertices*offset_bb_wrt_geom;
+
+    box.link_H_geometry = Transform(link_H_vertices.getRotation(),
+                                    offset_bb_wrt_link);
+
+    return box;
+}
+
+bool BBFromExternalShape(ExternalMesh* extMesh, Box& box)
+{
+    // Load mesh with assimp
+    Assimp::Importer Importer;
+
+    const aiScene* pScene = Importer.ReadFile(extMesh->filename.c_str(), 0);
+
+    if (pScene)
+    {
+        // Extract vector of vertices
+        double scale = 1.0;
+        std::vector<aiVector3D> vertexVector;
+        buildMesh(pScene, pScene->mRootNode, scale, vertexVector);
+
+        if (vertexVector.size() == 0)
+        {
+            return false;
+        }
+
+        box = extractAABBFromVertices(extMesh->link_H_geometry, toiDynTree(vertexVector));
+
+        return true;
+    }
+    else
+    {
+        std::stringstream ss;
+        ss << "Impossible to load mesh " << extMesh->filename << " using the Assimp library.";
+        reportError("", "BBFromExternalShape", ss.str().c_str());
+        return false;
+    }
+}
+
+bool BBFromShape(SolidShape* geom, Box& box)
+{
+    // Extract BB from shape, this would be benefic from being moved as a method in the SolidShape interface
+    if (geom->isBox())
+    {
+        // If shape is a box, just copy it
+        Box* pBox = static_cast<Box*>(geom);
+        box = *pBox;
+        box.link_H_geometry = geom->link_H_geometry;
+        return true;
+    }
+
+    if (geom->isSphere())
+    {
+        // If shape is a sphere all the side of the BB are the diameter of the sphere
+        Sphere* pSphere = static_cast<Sphere*>(geom);
+        box.x = box.y = box.z = 2.0*pSphere->radius;
+        box.link_H_geometry = geom->link_H_geometry;
+        return true;
+    }
+
+    if (geom->isCylinder())
+    {
+        // If shape is a cylinder the x and y side of the BB are the diameter of the cylinder,
+        // while the z side is the lenght of the cylinder
+        Cylinder* pCylinder = static_cast<Cylinder*>(geom);
+        box.x = box.y = 2.0*pCylinder->radius;
+        box.z = pCylinder->length;
+        box.link_H_geometry = geom->link_H_geometry;
+        return true;
+    }
+
+    if (geom->isExternalMesh())
+    {
+        // If shape is an external mesh, we need to load the mesh and extract the BB
+        ExternalMesh* pExtMesh = static_cast<ExternalMesh*>(geom);
+        return BBFromExternalShape(pExtMesh, box);
+    }
+
+    return false;
+}
+
+/**
+ * Compute vertices of the bounding box, computed in link frame
+ */
+std::vector<Position> computeBoxVertices(const Box box)
+{
+    std::vector<Position> vertices;
+
+    // + + +
+    vertices.push_back(box.link_H_geometry*Position(+box.x/2, +box.y/2, +box.z/2));
+
+    // + + -
+    vertices.push_back(box.link_H_geometry*Position(+box.x/2, +box.y/2, -box.z/2));
+
+    // + - +
+    vertices.push_back(box.link_H_geometry*Position(+box.x/2, -box.y/2, +box.z/2));
+
+    // + - -
+    vertices.push_back(box.link_H_geometry*Position(+box.x/2, -box.y/2, -box.z/2));
+
+    // - + +
+    vertices.push_back(box.link_H_geometry*Position(-box.x/2, +box.y/2, +box.z/2));
+
+    // - + -
+    vertices.push_back(box.link_H_geometry*Position(-box.x/2, +box.y/2, -box.z/2));
+
+    // - - +
+    vertices.push_back(box.link_H_geometry*Position(-box.x/2, -box.y/2, +box.z/2));
+
+    // - - -
+    vertices.push_back(box.link_H_geometry*Position(-box.x/2, -box.y/2, -box.z/2));
+
+    return vertices;
+}
+
+/**
+ * Compute the axis aligned bounding box out of a vector of axis aligned bounding boxes.
+ */
+Box computeAABoundingBox(const std::vector<Box>& shapesBBsInLinkFrame)
+{
+    // First compute the total set of vertices
+    std::vector<Position> BBVertices;
+    for (auto&& singleShapeBB : shapesBBsInLinkFrame) {
+        std::vector<Position> singleShapeBBVertices = computeBoxVertices(singleShapeBB);
+
+        BBVertices.insert(BBVertices.end(),
+                          singleShapeBBVertices.begin(),
+                          singleShapeBBVertices.end());
+    }
+
+    // Then extract the AABB of the combined vertices
+    return extractAABBFromVertices(Transform::Identity(), BBVertices);
+}
+
+
+bool getBoundingBoxOfLinkGeometries(iDynTree::Model& model,
+                                    std::vector<iDynTree::Box>& linkBBsInLinkFrame)
+{
+    linkBBsInLinkFrame.resize(model.getNrOfLinks());
+
+    for (LinkIndex lnkIdx=0; lnkIdx < model.getNrOfLinks(); lnkIdx++)
+    {
+        // If models has no shape associated
+        if (model.collisionSolidShapes().linkSolidShapes[lnkIdx].size() == 0)
+        {
+            Box box;
+            box.x = box.y = box.z = 0.0;
+            linkBBsInLinkFrame[lnkIdx] = box;
+            continue;
+        }
+
+        // Bounding boxes for each shape of the link, each expressed in link frame
+        std::vector<iDynTree::Box> shapesBBsInLinkFrame;
+        for(int shapeIdx=0; shapeIdx < model.collisionSolidShapes().linkSolidShapes[lnkIdx].size(); shapeIdx++)
+        {
+            iDynTree::Box shapeBoundingBox;
+            SolidShape * shape = model.collisionSolidShapes().linkSolidShapes[lnkIdx][shapeIdx];
+            bool ok = BBFromShape(shape, shapeBoundingBox);
+            if (!ok) return false;
+            shapesBBsInLinkFrame.push_back(shapeBoundingBox);
+        }
+
+        // Compute resulting AABB
+        linkBBsInLinkFrame[lnkIdx] = computeAABoundingBox(shapesBBsInLinkFrame);
+    }
+
+    return true;
+}
+#endif
+
+bool estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(const double totalMass,
+                                                                 iDynTree::Model& model,
+                                                                 VectorDynSize& estimatedInertialParams)
+{
+#ifdef IDYNTREE_USES_ASSIMP
+    // Resize the result vector
+    const int NR_OF_INERTIAL_PARAMS_FOR_LINK = 10;
+    const int nrOfInertialParametersOfModel = NR_OF_INERTIAL_PARAMS_FOR_LINK*model.getNrOfLinks();
+    estimatedInertialParams.resize(nrOfInertialParametersOfModel);
+
+    // Resize some internal buffers
+    std::vector<iDynTree::Box> boundingBoxVolume(model.getNrOfLinks());
+    std::vector<iDynTree::SpatialInertia> linkInertias(model.getNrOfLinks());
+
+    // Compute the bounding box for each link
+    bool ok = getBoundingBoxOfLinkGeometries(model, boundingBoxVolume);
+    if (!ok) return false;
+
+    double totalModelBBVolume = 0.0;
+    for (LinkIndex lnkIdx=0; lnkIdx < model.getNrOfLinks(); lnkIdx++)
+    {
+        totalModelBBVolume += boxGetVolume(boundingBoxVolume[lnkIdx]);
+    }
+
+    // Compute mass for each link
+    // Assume constant density for the robot
+    double density = totalMass/totalModelBBVolume;
+    for (LinkIndex lnkIdx=0; lnkIdx < model.getNrOfLinks(); lnkIdx++)
+    {
+        linkInertias[lnkIdx] = boxGet6DInertiaInLinkFrameFromDensity(boundingBoxVolume[lnkIdx], density);
+    }
+
+    // Convert inertias to vector
+    for(LinkIndex linkIdx = 0; linkIdx < model.getNrOfLinks(); linkIdx++ )
+    {
+        Vector10       inertiaParamsBuf = linkInertias[linkIdx].asVector();
+        toEigen(estimatedInertialParams).segment<10>(10*linkIdx) = toEigen(inertiaParamsBuf);
+    }
+
+    return true;
+#else
+    reportError("", "estimateInertialParametersFromLinkBoundingBoxesAndTotalMass", "IDYNTREE_USES_ASSIMP CMake option need to be set to ON to use estimateInertialParametersFromLinkBoundingBoxesAndTotalMass");
+    return false;
+#endif
+}
+
+}
diff --git a/src/solid-shapes/tests/CMakeLists.txt b/src/solid-shapes/tests/CMakeLists.txt
new file mode 100644
index 00000000000..efc3b2327a6
--- /dev/null
+++ b/src/solid-shapes/tests/CMakeLists.txt
@@ -0,0 +1,24 @@
+# Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
+#
+# Licensed under either the GNU Lesser General Public License v3.0 :
+# https://www.gnu.org/licenses/lgpl-3.0.html
+# or the GNU Lesser General Public License v2.1 :
+# https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+# at your option.
+
+macro(add_unit_test classname)
+    set(testsrc ${classname}IntegrationTest.cpp)
+    set(testbinary ${classname}UnitTest)
+    set(testname   UnitTest${classname})
+    add_executable(${testbinary} ${testsrc})
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-solid-shapes)
+    add_test(NAME ${testname} COMMAND ${testbinary})
+
+    if(IDYNTREE_RUN_VALGRIND_TESTS)
+        add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${testbinary}>)
+
+    endif()
+endmacro()
+
+add_unit_test(InertialParametersSolidShapesHelpers)
diff --git a/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp b/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp
new file mode 100644
index 00000000000..523a850bc27
--- /dev/null
+++ b/src/solid-shapes/tests/InertialParametersSolidShapesHelpersIntegrationTest.cpp
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Core/TestUtils.h>
+
+
+#include <iDynTree/InertialParametersSolidShapesHelpers.h>
+
+#include <iDynTree/Core/VectorDynSize.h>
+#include <iDynTree/Model/Model.h>
+
+
+using namespace iDynTree;
+
+void checkOneCubeVsEightSmallCubes()
+{
+    // Create a model with one shape with one cube of 1 meter of side,
+    // and another with 8 with 1/2 meter of side, placed to form a bigger cube
+    // As the density is linear, verify that given the same mass the estimated inertial parameters are
+    // the same
+
+    iDynTree::Model oneCubeModel;
+    iDynTree::Link oneCubeLink;
+    oneCubeModel.addLink("link0", oneCubeLink);
+    oneCubeModel.collisionSolidShapes().linkSolidShapes[0].resize(1);
+    iDynTree::Box oneCube;
+    oneCube.link_H_geometry = Transform::Identity();
+    oneCube.x = 1;
+    oneCube.y = 1;
+    oneCube.z = 1;
+    oneCubeModel.collisionSolidShapes().linkSolidShapes[0][0] = new iDynTree::Box(oneCube);
+
+    iDynTree::Model eightCubeModel;
+    iDynTree::Link eightCubeLink;
+    eightCubeModel.addLink("link0", eightCubeLink);
+    oneCubeModel.collisionSolidShapes().linkSolidShapes[0];
+
+    iDynTree::Box smallCube;
+    smallCube.link_H_geometry = Transform::Identity();
+    smallCube.x = 1.0/2.0;
+    smallCube.y = 1.0/2.0;
+    smallCube.z = 1.0/2.0;
+
+    // Generate origins for the small cubes
+    std::vector<Position> smallCubesOrigins;
+    // + + +
+    smallCubesOrigins.push_back(Position(+1.0/4.0, +1.0/4.0, +1.0/4.0));
+
+    // + + -
+    smallCubesOrigins.push_back(Position(+1.0/4.0, +1.0/4.0, -1.0/4.0));
+
+    // + - +
+    smallCubesOrigins.push_back(Position(+1.0/4.0, -1.0/4.0, +1.0/4.0));
+
+    // + - -
+    smallCubesOrigins.push_back(Position(+1.0/4.0, -1.0/4.0, -1.0/4.0));
+
+    // - + +
+    smallCubesOrigins.push_back(Position(-1.0/4.0, +1.0/4.0, +1.0/4.0));
+
+    // - + -
+    smallCubesOrigins.push_back(Position(-1.0/4.0, +1.0/4.0, -1.0/4.0));
+
+    // - - +
+    smallCubesOrigins.push_back(Position(-1.0/4.0, -1.0/4.0, +1.0/4.0));
+
+    // - - -
+    smallCubesOrigins.push_back(Position(-1.0/4.0, -1.0/4.0, -1.0/4.0));
+
+    for(auto&& smallCubeOrigin: smallCubesOrigins) {
+        smallCube.link_H_geometry.setPosition(smallCubeOrigin);
+        eightCubeModel.collisionSolidShapes().linkSolidShapes[0].push_back(new Box(smallCube));
+    }
+
+    // Compute the inertia with the bounding boxes for both cases
+    double totalMass = 1.0;
+    VectorDynSize oneCubeParams(oneCubeModel.getNrOfLinks()*10), eightCubesParams(oneCubeModel.getNrOfLinks()*10);
+    bool ok = estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(totalMass,
+                                                                          oneCubeModel,
+                                                                          oneCubeParams);
+    ASSERT_IS_TRUE(ok);
+
+    ok = estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(totalMass,
+                                                                     eightCubeModel,
+                                                                     eightCubesParams);
+    ASSERT_IS_TRUE(ok);
+
+    ASSERT_EQUAL_VECTOR(oneCubeParams, eightCubesParams);
+
+    std::cerr << "oneCubeParams: " << oneCubeParams.toString() << std::endl;
+    std::cerr << "eightCubesParams: " << eightCubesParams.toString() << std::endl;
+
+    // The first parameter is the mass
+    ASSERT_EQUAL_DOUBLE(oneCubeParams(0), totalMass);
+}
+
+
+int main()
+{
+    checkOneCubeVsEightSmallCubes();
+
+    return EXIT_SUCCESS;
+}
+
diff --git a/src/tests/data/cube.stl b/src/tests/data/cube.stl
new file mode 100644
index 00000000000..8c535a9b6e9
--- /dev/null
+++ b/src/tests/data/cube.stl
@@ -0,0 +1,86 @@
+solid ascii
+   facet normal 0.000000 1.000000 0.000000
+     outer loop
+       vertex 1.000000 1.000000 1.000000
+       vertex 1.000000 1.000000 -1.000000
+       vertex -1.000000 1.000000 -1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 1.000000 0.000000
+     outer loop
+       vertex 1.000000 1.000000 1.000000
+       vertex -1.000000 1.000000 -1.000000
+       vertex -1.000000 1.000000 1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 0.000000 1.000000
+     outer loop
+       vertex 1.000000 1.000000 1.000000
+       vertex -1.000000 1.000000 1.000000
+       vertex -1.000000 -1.000000 1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 0.000000 1.000000
+     outer loop
+       vertex 1.000000 1.000000 1.000000
+       vertex -1.000000 -1.000000 1.000000
+       vertex 1.000000 -1.000000 1.000000
+     endloop
+   endfacet
+   facet normal 1.000000 0.000000 0.000000
+     outer loop
+       vertex 1.000000 1.000000 1.000000
+       vertex 1.000000 -1.000000 1.000000
+       vertex 1.000000 -1.000000 -1.000000
+     endloop
+   endfacet
+   facet normal 1.000000 0.000000 0.000000
+     outer loop
+       vertex 1.000000 1.000000 1.000000
+       vertex 1.000000 -1.000000 -1.000000
+       vertex 1.000000 1.000000 -1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 0.000000 -1.000000
+     outer loop
+       vertex -1.000000 -1.000000 -1.000000
+       vertex 1.000000 1.000000 -1.000000
+       vertex 1.000000 -1.000000 -1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 -1.000000 0.000000
+     outer loop
+       vertex -1.000000 -1.000000 -1.000000
+       vertex 1.000000 -1.000000 -1.000000
+       vertex 1.000000 -1.000000 1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 -1.000000 0.000000
+     outer loop
+       vertex -1.000000 -1.000000 -1.000000
+       vertex 1.000000 -1.000000 1.000000
+       vertex -1.000000 -1.000000 1.000000
+     endloop
+   endfacet
+   facet normal -1.000000 0.000000 0.000000
+     outer loop
+       vertex -1.000000 -1.000000 -1.000000
+       vertex -1.000000 -1.000000 1.000000
+       vertex -1.000000 1.000000 1.000000
+     endloop
+   endfacet
+   facet normal -1.000000 0.000000 0.000000
+     outer loop
+       vertex -1.000000 -1.000000 -1.000000
+       vertex -1.000000 1.000000 1.000000
+       vertex -1.000000 1.000000 -1.000000
+     endloop
+   endfacet
+   facet normal 0.000000 0.000000 -1.000000
+     outer loop
+       vertex -1.000000 -1.000000 -1.000000
+       vertex -1.000000 1.000000 -1.000000
+       vertex 1.000000 1.000000 -1.000000
+     endloop
+   endfacet
+endsolid
\ No newline at end of file
diff --git a/src/tests/data/oneLink.urdf b/src/tests/data/oneLink.urdf
index 153910c4849..e58ab707945 100644
--- a/src/tests/data/oneLink.urdf
+++ b/src/tests/data/oneLink.urdf
@@ -11,6 +11,18 @@
             <mass value="1" />
             <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
         </inertial>
+        <visual>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="cube.stl"/>
+          </geometry>
+        </visual>
+        <collision>
+          <origin xyz="0 0 0" rpy="0 0 0" />
+          <geometry>
+            <mesh filename="cube.stl"/>
+          </geometry>
+        </collision>
     </link>
     <!-- frames , modeled as "fake links" -->
      <joint name="frame1_fixed_joint" type="fixed">
diff --git a/src/tests/integration/CMakeLists.txt b/src/tests/integration/CMakeLists.txt
index fcca1bd131c..59ea85281d0 100644
--- a/src/tests/integration/CMakeLists.txt
+++ b/src/tests/integration/CMakeLists.txt
@@ -14,8 +14,10 @@ macro(add_integration_test testname)
     set(testtarget   IntegrationTest${testname})
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation)
-    add_test(NAME ${testtarget} COMMAND ${testbinary})
+    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation idyntree-solid-shapes)
+    # Setting explicitly the WORKING_DIRECTORY is necessary to make sure that meshes are correctly loaded,
+    # as a workaround for https://github.com/robotology/idyntree/issues/291
+    add_test(NAME ${testtarget} COMMAND ${testbinary} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/src/tests/data)
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
         add_test(NAME memcheck_${testtarget} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${testbinary}>)
@@ -28,7 +30,7 @@ macro(add_integration_test_no_valgrind testname)
     set(testtarget   IntegrationTest${testname})
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation)
+    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation  idyntree-solid-shapes)
     add_test(NAME ${testtarget} COMMAND ${testbinary})
 endmacro()
 
@@ -49,3 +51,6 @@ add_integration_test_no_valgrind(iCubTorqueEstimation)
 
 # Until we fix it, add DynamicsLinearization test but don't execute it
 add_integration_exe(DynamicsLinearization)
+
+
+add_integration_test(InertialParametersSolidShapesHelpers)
diff --git a/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp b/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp
new file mode 100644
index 00000000000..90444d4ff84
--- /dev/null
+++ b/src/tests/integration/InertialParametersSolidShapesHelpersIntegrationTest.cpp
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <iDynTree/Core/TestUtils.h>
+
+#include "testModels.h"
+
+#include <iDynTree/InertialParametersSolidShapesHelpers.h>
+#include <iDynTree/ModelIO/ModelLoader.h>
+
+
+using namespace iDynTree;
+
+int main()
+{
+    // Open file with a external mesh (cube centered in 0 and with vertices in (1,0,0), (0,0,1), etc etc
+    std::string urdfFileName = getAbsModelPath("oneLink.urdf");
+
+    // Load model
+    iDynTree::ModelLoader mdlLoader;
+    bool ok = mdlLoader.loadModelFromFile(urdfFileName);
+
+    ASSERT_IS_TRUE(ok);
+
+    // Ensure that the model has one mesh
+    Model model = mdlLoader.model();
+
+    //ASSERT_IS_TRUE(model.collisionSolidShapes().linkSolidShapes[0].size() ==  1);
+
+    // Extract the inertial parameters assuming a total mass of 24 Kg
+    // The principal moment of inertia for a cube of 2 meters side will
+    // be (density is 24/8 = 3 Kg/m^3) of 2*24/3 = 16 .
+    // http://scienceworld.wolfram.com/physics/MomentofInertiaRectangularParallelepiped.html
+    double totalMass = 24;
+    double expectedInertia = 16;
+    RotationalInertiaRaw rotInertia;
+    rotInertia.zero();
+    rotInertia(0, 0) = rotInertia(1, 1) = rotInertia(2, 2) = expectedInertia;
+
+
+    VectorDynSize inertialParams;
+    inertialParams.resize(10);
+    inertialParams(2) = 3.0;
+    ok = estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(totalMass, model, inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    std::cerr << inertialParams.toString() << std::endl;
+
+    // Update the inertial parameters of the model with the one
+    ok = model.updateInertialParameters(inertialParams);
+    ASSERT_IS_TRUE(ok);
+
+    // Check the inertial params
+    iDynTree::SpatialInertia inertia = model.getLink(0)->getInertia();
+
+        std::cerr << inertia.getMass() << std::endl;
+        std::cerr << totalMass << std::endl;
+
+    ASSERT_EQUAL_DOUBLE(inertia.getMass(), totalMass);
+    Position comZero;
+    comZero.zero();
+    ASSERT_EQUAL_VECTOR(inertia.getCenterOfMass(), comZero);
+    std::cerr << inertia.getRotationalInertiaWrtFrameOrigin().toString() << std::endl;
+    std::cerr << rotInertia.toString() << std::endl;
+
+    ASSERT_EQUAL_MATRIX(inertia.getRotationalInertiaWrtFrameOrigin(), rotInertia);
+
+
+    std::cerr << "Test successful." << std::endl;
+    return EXIT_SUCCESS;
+}

From eb222a88a93215534b0eb15f1df6fcb6bf676651 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 18 Sep 2019 10:12:01 +0200
Subject: [PATCH 147/194] Add release notes and disable tests if
 IDYNTREE_USES_ASSIMP is False

---
 doc/releases/v0_12.md                | 10 ++++++++++
 src/solid-shapes/CMakeLists.txt      |  2 +-
 src/tests/integration/CMakeLists.txt |  5 +++--
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index fbb2d60d1ce..66cecb4ec86 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -32,6 +32,16 @@ KinDynComputations finally reached feature parity with respect to DynamicsComput
 #### `visualization`
 * Added visualization of vectors.
 
+#### `solid-shapes`
+* Added a SolidShape helper library. This library is part of iDynTree, and is meant
+to contain all the algorithms that use in some form the visual and collision geometries of the model,
+and so they depend on the Assimp library to load meshes.
+* Added an helper function that provides rough estimates of the inertial parameters (mass, first moments of mass,
+3d inertia matrix elements) of a robot given the total mass of the robot, and its collisions shapes. While the estimates
+provided are quite rough, they can be quite useful at least to provide an expected order of magnitude of the parameters,
+to normalize errors or as initial points of a nonlinear optimization procedure.
+
+
 #### `estimation`
 * Added attitude estimator interface to estimate the orientation of an IMU, given the IMU measurements
 * Added `DiscreteExtendedKalmanFilterHelper` base class
diff --git a/src/solid-shapes/CMakeLists.txt b/src/solid-shapes/CMakeLists.txt
index 36a5d19c576..62453096344 100644
--- a/src/solid-shapes/CMakeLists.txt
+++ b/src/solid-shapes/CMakeLists.txt
@@ -34,6 +34,6 @@ set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
 
 
-if(IDYNTREE_COMPILE_TESTS)
+if(IDYNTREE_COMPILE_TESTS AND IDYNTREE_USES_ASSIMP)
     add_subdirectory(tests)
 endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/tests/integration/CMakeLists.txt b/src/tests/integration/CMakeLists.txt
index 59ea85281d0..cf6c4ac5e6c 100644
--- a/src/tests/integration/CMakeLists.txt
+++ b/src/tests/integration/CMakeLists.txt
@@ -52,5 +52,6 @@ add_integration_test_no_valgrind(iCubTorqueEstimation)
 # Until we fix it, add DynamicsLinearization test but don't execute it
 add_integration_exe(DynamicsLinearization)
 
-
-add_integration_test(InertialParametersSolidShapesHelpers)
+if(IDYNTREE_USES_ASSIMP)
+  add_integration_test(InertialParametersSolidShapesHelpers)
+endif()

From 017a921729a4e9cd51001570f364a3817c5fdad0 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 18 Sep 2019 10:29:47 +0200
Subject: [PATCH 148/194] Add support for handling scale in
 InertialParametersSolidShapesHelpers

Originally contributed in https://github.com/robotology/idyntree/pull/577/files
---
 .../InertialParametersSolidShapesHelpers.cpp  | 13 ++++
 src/tests/data/cube.stl                       | 74 +++++++++----------
 src/tests/data/oneLink.urdf                   |  4 +-
 3 files changed, 52 insertions(+), 39 deletions(-)

diff --git a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp
index 6c7355e466e..398cd188bda 100644
--- a/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp
+++ b/src/solid-shapes/src/InertialParametersSolidShapesHelpers.cpp
@@ -199,6 +199,19 @@ bool BBFromExternalShape(ExternalMesh* extMesh, Box& box)
             return false;
         }
 
+        // Apply scale attribute for external meshes
+        double scalingFactorX=extMesh->scale.getVal(0);
+        double scalingFactorY=extMesh->scale.getVal(1);
+        double scalingFactorZ=extMesh->scale.getVal(2);
+
+        // Use each component to scale each vertex coordinate
+        for(size_t i=0; i < vertexVector.size(); i++)
+        {
+            vertexVector[i].x*=scalingFactorX;
+            vertexVector[i].y*=scalingFactorY;
+            vertexVector[i].z*=scalingFactorZ;
+        }
+
         box = extractAABBFromVertices(extMesh->link_H_geometry, toiDynTree(vertexVector));
 
         return true;
diff --git a/src/tests/data/cube.stl b/src/tests/data/cube.stl
index 8c535a9b6e9..b84380d430b 100644
--- a/src/tests/data/cube.stl
+++ b/src/tests/data/cube.stl
@@ -1,86 +1,86 @@
 solid ascii
    facet normal 0.000000 1.000000 0.000000
      outer loop
-       vertex 1.000000 1.000000 1.000000
-       vertex 1.000000 1.000000 -1.000000
-       vertex -1.000000 1.000000 -1.000000
+       vertex 10.000000 10.000000 10.000000
+       vertex 10.000000 10.000000 -10.000000
+       vertex -10.000000 10.000000 -10.000000
      endloop
    endfacet
    facet normal 0.000000 1.000000 0.000000
      outer loop
-       vertex 1.000000 1.000000 1.000000
-       vertex -1.000000 1.000000 -1.000000
-       vertex -1.000000 1.000000 1.000000
+       vertex 10.000000 10.000000 10.000000
+       vertex -10.000000 10.000000 -10.000000
+       vertex -10.000000 10.000000 10.000000
      endloop
    endfacet
    facet normal 0.000000 0.000000 1.000000
      outer loop
-       vertex 1.000000 1.000000 1.000000
-       vertex -1.000000 1.000000 1.000000
-       vertex -1.000000 -1.000000 1.000000
+       vertex 10.000000 10.000000 10.000000
+       vertex -10.000000 10.000000 10.000000
+       vertex -10.000000 -10.000000 10.000000
      endloop
    endfacet
    facet normal 0.000000 0.000000 1.000000
      outer loop
-       vertex 1.000000 1.000000 1.000000
-       vertex -1.000000 -1.000000 1.000000
-       vertex 1.000000 -1.000000 1.000000
+       vertex 10.000000 10.000000 10.000000
+       vertex -10.000000 -10.000000 10.000000
+       vertex 10.000000 -10.000000 10.000000
      endloop
    endfacet
    facet normal 1.000000 0.000000 0.000000
      outer loop
-       vertex 1.000000 1.000000 1.000000
-       vertex 1.000000 -1.000000 1.000000
-       vertex 1.000000 -1.000000 -1.000000
+       vertex 10.000000 10.000000 10.000000
+       vertex 10.000000 -10.000000 10.000000
+       vertex 10.000000 -10.000000 -10.000000
      endloop
    endfacet
    facet normal 1.000000 0.000000 0.000000
      outer loop
-       vertex 1.000000 1.000000 1.000000
-       vertex 1.000000 -1.000000 -1.000000
-       vertex 1.000000 1.000000 -1.000000
+       vertex 10.000000 10.000000 10.000000
+       vertex 10.000000 -10.000000 -10.000000
+       vertex 10.000000 10.000000 -10.000000
      endloop
    endfacet
    facet normal 0.000000 0.000000 -1.000000
      outer loop
-       vertex -1.000000 -1.000000 -1.000000
-       vertex 1.000000 1.000000 -1.000000
-       vertex 1.000000 -1.000000 -1.000000
+       vertex -10.000000 -10.000000 -10.000000
+       vertex 10.000000 10.000000 -10.000000
+       vertex 10.000000 -10.000000 -10.000000
      endloop
    endfacet
    facet normal 0.000000 -1.000000 0.000000
      outer loop
-       vertex -1.000000 -1.000000 -1.000000
-       vertex 1.000000 -1.000000 -1.000000
-       vertex 1.000000 -1.000000 1.000000
+       vertex -10.000000 -10.000000 -10.000000
+       vertex 10.000000 -10.000000 -10.000000
+       vertex 10.000000 -10.000000 10.000000
      endloop
    endfacet
    facet normal 0.000000 -1.000000 0.000000
      outer loop
-       vertex -1.000000 -1.000000 -1.000000
-       vertex 1.000000 -1.000000 1.000000
-       vertex -1.000000 -1.000000 1.000000
+       vertex -10.000000 -10.000000 -10.000000
+       vertex 10.000000 -10.000000 10.000000
+       vertex -10.000000 -10.000000 10.000000
      endloop
    endfacet
    facet normal -1.000000 0.000000 0.000000
      outer loop
-       vertex -1.000000 -1.000000 -1.000000
-       vertex -1.000000 -1.000000 1.000000
-       vertex -1.000000 1.000000 1.000000
+       vertex -10.000000 -10.000000 -10.000000
+       vertex -10.000000 -10.000000 10.000000
+       vertex -10.000000 10.000000 10.000000
      endloop
    endfacet
    facet normal -1.000000 0.000000 0.000000
      outer loop
-       vertex -1.000000 -1.000000 -1.000000
-       vertex -1.000000 1.000000 1.000000
-       vertex -1.000000 1.000000 -1.000000
+       vertex -10.000000 -10.000000 -10.000000
+       vertex -10.000000 10.000000 10.000000
+       vertex -10.000000 10.000000 -10.000000
      endloop
    endfacet
    facet normal 0.000000 0.000000 -1.000000
      outer loop
-       vertex -1.000000 -1.000000 -1.000000
-       vertex -1.000000 1.000000 -1.000000
-       vertex 1.000000 1.000000 -1.000000
+       vertex -10.000000 -10.000000 -10.000000
+       vertex -10.000000 10.000000 -10.000000
+       vertex 10.000000 10.000000 -10.000000
      endloop
    endfacet
-endsolid
\ No newline at end of file
+endsolid
diff --git a/src/tests/data/oneLink.urdf b/src/tests/data/oneLink.urdf
index e58ab707945..aa9670ce31b 100644
--- a/src/tests/data/oneLink.urdf
+++ b/src/tests/data/oneLink.urdf
@@ -14,13 +14,13 @@
         <visual>
           <origin xyz="0 0 0" rpy="0 0 0" />
           <geometry>
-            <mesh filename="cube.stl"/>
+            <mesh filename="cube.stl" scale="0.1 0.1 0.1"/>
           </geometry>
         </visual>
         <collision>
           <origin xyz="0 0 0" rpy="0 0 0" />
           <geometry>
-            <mesh filename="cube.stl"/>
+            <mesh filename="cube.stl" scale="0.1 0.1 0.1"/>
           </geometry>
         </collision>
     </link>

From 1ec37f8ad391bf3dda08cd7fe66599d3807d483b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 19 Sep 2019 16:21:35 +0200
Subject: [PATCH 149/194] Add ModelExporter and
 InertialParametersSolidShapesHelpers to the bindings

---
 bindings/iDynTree.i | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i
index e47c756758d..92ea79ec347 100644
--- a/bindings/iDynTree.i
+++ b/bindings/iDynTree.i
@@ -108,6 +108,7 @@
 #include "iDynTree/ModelIO/URDFDofsImport.h"
 #include "iDynTree/ModelIO/URDFGenericSensorsImport.h"
 #include "iDynTree/ModelIO/ModelLoader.h"
+#include "iDynTree/ModelIO/ModelExporter.h"
 
 // Estimation related classes
 #include "iDynTree/Estimation/ExternalWrenchesEstimation.h"
@@ -120,6 +121,9 @@
 #include "iDynTree/Estimation/ExtendedKalmanFilter.h"
 #include "iDynTree/Estimation/AttitudeQuaternionEKF.h"
 
+// SolidShapes related classes
+#include "iDynTree/InertialParametersSolidShapesHelpers.h"
+
 // Regressors related data structures
 #include "iDynTree/Regressors/DynamicsRegressorParameters.h"
 #include "iDynTree/Regressors/DynamicsRegressorGenerator.h"
@@ -312,6 +316,7 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 %include "iDynTree/ModelIO/URDFDofsImport.h"
 %include "iDynTree/ModelIO/URDFGenericSensorsImport.h"
 %include "iDynTree/ModelIO/ModelLoader.h"
+%include "iDynTree/ModelIO/ModelExporter.h"
 
 // Estimation related classes
 %include "iDynTree/Estimation/ExternalWrenchesEstimation.h"
@@ -324,6 +329,9 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 %include "iDynTree/Estimation/ExtendedKalmanFilter.h"
 %include "iDynTree/Estimation/AttitudeQuaternionEKF.h"
 
+// SolidShapes related classes
+%include "iDynTree/InertialParametersSolidShapesHelpers.h"
+
 // Regressors related data structures
 %include "iDynTree/Regressors/DynamicsRegressorParameters.h"
 %include "iDynTree/Regressors/DynamicsRegressorGenerator.h"

From 10d2856a85b2b1406e693fe8b0625522e8c43bf3 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 19 Sep 2019 16:22:55 +0200
Subject: [PATCH 150/194] [bindings] update matlab bindings

---
 .../+iDynTree/AccelerometerSensor.m           |   32 +-
 .../+iDynTree/ArticulatedBodyAlgorithm.m      |    2 +-
 .../ArticulatedBodyAlgorithmInternalBuffers.m |   44 +-
 .../+iDynTree/AttitudeEstimatorState.m        |   16 +-
 .../+iDynTree/AttitudeMahonyFilter.m          |   40 +-
 .../AttitudeMahonyFilterParameters.m          |   24 +-
 .../+iDynTree/AttitudeQuaternionEKF.m         |   44 +-
 .../AttitudeQuaternionEKFParameters.m         |   44 +-
 .../+iDynTree/BerdyDynamicVariable.m          |   20 +-
 .../autogenerated/+iDynTree/BerdyHelper.m     |   64 +-
 .../autogenerated/+iDynTree/BerdyOptions.m    |   38 +-
 .../autogenerated/+iDynTree/BerdySensor.m     |   20 +-
 .../+iDynTree/BerdySparseMAPSolver.m          |   32 +-
 bindings/matlab/autogenerated/+iDynTree/Box.m |   18 +-
 .../matlab/autogenerated/+iDynTree/ColorViz.m |   20 +-
 .../+iDynTree/CompositeRigidBodyAlgorithm.m   |    2 +-
 .../ComputeLinearAndAngularMomentum.m         |    2 +-
 ...teLinearAndAngularMomentumDerivativeBias.m |    2 +-
 .../autogenerated/+iDynTree/ContactWrench.m   |   10 +-
 .../matlab/autogenerated/+iDynTree/Cylinder.m |   14 +-
 .../+iDynTree/DOFSpatialForceArray.m          |   10 +-
 .../+iDynTree/DOFSpatialMotionArray.m         |   10 +-
 .../+iDynTree/DOF_INVALID_INDEX.m             |    4 +-
 .../+iDynTree/DOF_INVALID_NAME.m              |    4 +-
 .../DiscreteExtendedKalmanFilterHelper.m      |   40 +-
 .../autogenerated/+iDynTree/DynamicSpan.m     |   44 +-
 .../+iDynTree/DynamicsComputations.m          |   72 +-
 .../+iDynTree/DynamicsRegressorGenerator.m    |   60 +-
 .../+iDynTree/DynamicsRegressorParameter.m    |   22 +-
 .../DynamicsRegressorParametersList.m         |   18 +-
 .../ExtWrenchesAndJointTorquesEstimator.m     |   28 +-
 .../autogenerated/+iDynTree/ExternalMesh.m    |   14 +-
 .../+iDynTree/FRAME_INVALID_INDEX.m           |    4 +-
 .../+iDynTree/FRAME_INVALID_NAME.m            |    4 +-
 .../autogenerated/+iDynTree/FixedJoint.m      |   62 +-
 .../+iDynTree/ForwardAccKinematics.m          |    2 +-
 .../+iDynTree/ForwardBiasAccKinematics.m      |    2 +-
 .../+iDynTree/ForwardPosVelAccKinematics.m    |    2 +-
 .../+iDynTree/ForwardPosVelKinematics.m       |    2 +-
 .../+iDynTree/ForwardPositionKinematics.m     |    2 +-
 .../+iDynTree/ForwardVelAccKinematics.m       |    2 +-
 .../+iDynTree/FrameFreeFloatingJacobian.m     |    8 +-
 .../autogenerated/+iDynTree/FreeFloatingAcc.m |   12 +-
 .../FreeFloatingGeneralizedTorques.m          |   12 +-
 .../+iDynTree/FreeFloatingMassMatrix.m        |    6 +-
 .../autogenerated/+iDynTree/FreeFloatingPos.m |   12 +-
 .../autogenerated/+iDynTree/FreeFloatingVel.m |   12 +-
 .../autogenerated/+iDynTree/GyroscopeSensor.m |   32 +-
 .../+iDynTree/IAttitudeEstimator.m            |   22 +-
 .../matlab/autogenerated/+iDynTree/ICamera.m  |    8 +-
 .../autogenerated/+iDynTree/IEnvironment.m    |   18 +-
 .../+iDynTree/IJetsVisualization.m            |   16 +-
 .../matlab/autogenerated/+iDynTree/IJoint.m   |   68 +-
 .../matlab/autogenerated/+iDynTree/ILight.m   |   28 +-
 .../+iDynTree/IModelVisualization.m           |   34 +-
 .../+iDynTree/IVectorsVisualization.m         |   14 +-
 ...verseDynamicsInertialParametersRegressor.m |    2 +-
 .../+iDynTree/JOINT_INVALID_INDEX.m           |    4 +-
 .../+iDynTree/JOINT_INVALID_NAME.m            |    4 +-
 .../+iDynTree/JointDOFsDoubleArray.m          |    8 +-
 .../+iDynTree/JointPosDoubleArray.m           |    8 +-
 .../autogenerated/+iDynTree/JointSensor.m     |   12 +-
 .../+iDynTree/KinDynComputations.m            |  110 +-
 .../+iDynTree/LINK_INVALID_INDEX.m            |    4 +-
 .../+iDynTree/LINK_INVALID_NAME.m             |    4 +-
 .../matlab/autogenerated/+iDynTree/Link.m     |   14 +-
 .../autogenerated/+iDynTree/LinkAccArray.m    |   14 +-
 .../+iDynTree/LinkArticulatedBodyInertias.m   |   10 +-
 .../+iDynTree/LinkContactWrenches.m           |   18 +-
 .../autogenerated/+iDynTree/LinkInertias.m    |   10 +-
 .../autogenerated/+iDynTree/LinkPositions.m   |   14 +-
 .../autogenerated/+iDynTree/LinkSensor.m      |   16 +-
 .../+iDynTree/LinkUnknownWrenchContacts.m     |   22 +-
 .../autogenerated/+iDynTree/LinkVelArray.m    |   14 +-
 .../autogenerated/+iDynTree/LinkWrenches.m    |   16 +-
 .../matlab/autogenerated/+iDynTree/Model.m    |   84 +-
 .../autogenerated/+iDynTree/ModelExporter.m   |   50 +
 .../+iDynTree/ModelExporterOptions.m          |   46 +
 .../autogenerated/+iDynTree/ModelLoader.m     |   24 +-
 .../+iDynTree/ModelParserOptions.m            |   12 +-
 .../+iDynTree/ModelSolidShapes.m              |   14 +-
 .../+iDynTree/MomentumFreeFloatingJacobian.m  |    8 +-
 .../+iDynTree/MovableJointImpl1.m             |   18 +-
 .../+iDynTree/MovableJointImpl2.m             |   18 +-
 .../+iDynTree/MovableJointImpl3.m             |   18 +-
 .../+iDynTree/MovableJointImpl4.m             |   18 +-
 .../+iDynTree/MovableJointImpl5.m             |   18 +-
 .../+iDynTree/MovableJointImpl6.m             |   18 +-
 .../+iDynTree/NR_OF_SENSOR_TYPES.m            |    2 +-
 .../matlab/autogenerated/+iDynTree/Neighbor.m |   12 +-
 .../autogenerated/+iDynTree/PrismaticJoint.m  |   50 +-
 .../+iDynTree/RNEADynamicPhase.m              |    2 +-
 .../autogenerated/+iDynTree/RevoluteJoint.m   |   50 +-
 .../matlab/autogenerated/+iDynTree/Rotation.m |   28 +-
 .../matlab/autogenerated/+iDynTree/Sensor.m   |   18 +-
 .../autogenerated/+iDynTree/SensorsList.m     |   34 +-
 .../+iDynTree/SensorsMeasurements.m           |   18 +-
 .../+iDynTree/SimpleLeggedOdometry.m          |   25 +-
 .../+iDynTree/SixAxisForceTorqueSensor.m      |   60 +-
 .../autogenerated/+iDynTree/SolidShape.m      |   42 +-
 .../matlab/autogenerated/+iDynTree/Sphere.m   |   10 +-
 .../+iDynTree/TRAVERSAL_INVALID_INDEX.m       |    4 +-
 .../ThreeAxisAngularAccelerometerSensor.m     |   30 +-
 .../ThreeAxisForceTorqueContactSensor.m       |   36 +-
 .../autogenerated/+iDynTree/Transform.m       |   38 +-
 .../+iDynTree/TransformDerivative.m           |   26 +-
 .../+iDynTree/TransformSemantics.m            |   16 +-
 .../autogenerated/+iDynTree/Traversal.m       |   34 +-
 .../+iDynTree/URDFParserOptions.m             |   12 +-
 .../+iDynTree/UnknownWrenchContact.m          |   24 +-
 .../autogenerated/+iDynTree/Visualizer.m      |   30 +-
 .../+iDynTree/VisualizerOptions.m             |   20 +-
 .../computeLinkNetWrenchesWithoutGravity.m    |    2 +-
 .../+iDynTree/dofsListFromURDF.m              |    2 +-
 .../+iDynTree/dofsListFromURDFString.m        |    2 +-
 .../autogenerated/+iDynTree/dynamic_extent.m  |    2 +-
 ...ynamicsEstimationForwardVelAccKinematics.m |    2 +-
 .../dynamicsEstimationForwardVelKinematics.m  |    2 +-
 .../+iDynTree/estimateExternalWrenches.m      |    2 +-
 .../estimateExternalWrenchesBuffers.m         |   36 +-
 ...stimateExternalWrenchesWithoutInternalFT.m |    2 +-
 ...ametersFromLinkBoundingBoxesAndTotalMass.m |    3 +
 ...ntactWrenchesFromLinkNetExternalWrenches.m |    2 +-
 .../+iDynTree/getSensorTypeSize.m             |    2 +-
 .../+iDynTree/input_dimensions.m              |    2 +-
 .../+iDynTree/isDOFBerdyDynamicVariable.m     |    2 +-
 .../+iDynTree/isJointBerdyDynamicVariable.m   |    2 +-
 .../autogenerated/+iDynTree/isJointSensor.m   |    2 +-
 .../+iDynTree/isLinkBerdyDynamicVariable.m    |    2 +-
 .../autogenerated/+iDynTree/isLinkSensor.m    |    2 +-
 .../autogenerated/+iDynTree/modelFromURDF.m   |    2 +-
 .../+iDynTree/modelFromURDFString.m           |    2 +-
 .../output_dimensions_with_magnetometer.m     |    2 +-
 .../output_dimensions_without_magnetometer.m  |    2 +-
 .../+iDynTree/predictSensorsMeasurements.m    |    2 +-
 ...predictSensorsMeasurementsFromRawBuffers.m |    2 +-
 .../autogenerated/+iDynTree/sensorsFromURDF.m |    2 +-
 .../+iDynTree/sensorsFromURDFString.m         |    2 +-
 .../autogenerated/iDynTreeMATLAB_wrap.cxx     | 7261 ++++++++++-------
 139 files changed, 5853 insertions(+), 3964 deletions(-)
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/ModelExporter.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/ModelExporterOptions.m
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m

diff --git a/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m b/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m
index b6534e0eab2..edea6989df1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AccelerometerSensor.m
@@ -7,58 +7,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1369, varargin{:});
+        tmp = iDynTreeMEX(1377, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1370, self);
+        iDynTreeMEX(1378, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1371, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1379, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1372, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1380, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1373, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1381, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1374, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1382, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1375, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1383, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1376, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1384, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1377, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1385, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1378, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1386, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1379, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1387, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1380, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1388, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1381, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1389, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1382, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1390, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1383, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1391, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1384, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1392, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m
index e6133cbd920..a5ee5337e77 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithm.m
@@ -1,3 +1,3 @@
 function varargout = ArticulatedBodyAlgorithm(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1284, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1292, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m
index 28497e05a4b..329c8f0f8e6 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ArticulatedBodyAlgorithmInternalBuffers.m
@@ -9,110 +9,110 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1262, varargin{:});
+        tmp = iDynTreeMEX(1270, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1263, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1271, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1264, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1272, self, varargin{:});
     end
     function varargout = S(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1265, self);
+        varargout{1} = iDynTreeMEX(1273, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1266, self, varargin{1});
+        iDynTreeMEX(1274, self, varargin{1});
       end
     end
     function varargout = U(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1267, self);
+        varargout{1} = iDynTreeMEX(1275, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1268, self, varargin{1});
+        iDynTreeMEX(1276, self, varargin{1});
       end
     end
     function varargout = D(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1269, self);
+        varargout{1} = iDynTreeMEX(1277, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1270, self, varargin{1});
+        iDynTreeMEX(1278, self, varargin{1});
       end
     end
     function varargout = u(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1271, self);
+        varargout{1} = iDynTreeMEX(1279, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1272, self, varargin{1});
+        iDynTreeMEX(1280, self, varargin{1});
       end
     end
     function varargout = linksVel(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1273, self);
+        varargout{1} = iDynTreeMEX(1281, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1274, self, varargin{1});
+        iDynTreeMEX(1282, self, varargin{1});
       end
     end
     function varargout = linksBiasAcceleration(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1275, self);
+        varargout{1} = iDynTreeMEX(1283, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1276, self, varargin{1});
+        iDynTreeMEX(1284, self, varargin{1});
       end
     end
     function varargout = linksAccelerations(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1277, self);
+        varargout{1} = iDynTreeMEX(1285, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1278, self, varargin{1});
+        iDynTreeMEX(1286, self, varargin{1});
       end
     end
     function varargout = linkABIs(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1279, self);
+        varargout{1} = iDynTreeMEX(1287, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1280, self, varargin{1});
+        iDynTreeMEX(1288, self, varargin{1});
       end
     end
     function varargout = linksBiasWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1281, self);
+        varargout{1} = iDynTreeMEX(1289, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1282, self, varargin{1});
+        iDynTreeMEX(1290, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1283, self);
+        iDynTreeMEX(1291, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
index 36bf097e9ff..9a09409acd4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
@@ -7,30 +7,30 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1694, self);
+        varargout{1} = iDynTreeMEX(1653, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1695, self, varargin{1});
+        iDynTreeMEX(1654, self, varargin{1});
       end
     end
     function varargout = m_angular_velocity(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1696, self);
+        varargout{1} = iDynTreeMEX(1655, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1697, self, varargin{1});
+        iDynTreeMEX(1656, self, varargin{1});
       end
     end
     function varargout = m_gyroscope_bias(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1698, self);
+        varargout{1} = iDynTreeMEX(1657, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1699, self, varargin{1});
+        iDynTreeMEX(1658, self, varargin{1});
       end
     end
     function self = AttitudeEstimatorState(varargin)
@@ -39,14 +39,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1700, varargin{:});
+        tmp = iDynTreeMEX(1659, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1701, self);
+        iDynTreeMEX(1660, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
index 0c88c1ddd1e..2e3d1f16256 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
@@ -7,68 +7,68 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1651, varargin{:});
+        tmp = iDynTreeMEX(1684, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = useMagnetoMeterMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1652, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1685, self, varargin{:});
     end
     function varargout = setConfidenceForMagnetometerMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1653, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1686, self, varargin{:});
     end
     function varargout = setGainkp(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1654, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1687, self, varargin{:});
     end
     function varargout = setGainki(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1655, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1688, self, varargin{:});
     end
     function varargout = setTimeStepInSeconds(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1656, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1689, self, varargin{:});
     end
     function varargout = setGravityDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1657, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1690, self, varargin{:});
     end
     function varargout = setParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1658, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1691, self, varargin{:});
     end
     function varargout = getParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1659, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1692, self, varargin{:});
     end
     function varargout = updateFilterWithMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1660, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1693, self, varargin{:});
     end
     function varargout = propagateStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1661, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1694, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1662, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1695, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsQuaternion(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1663, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1696, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRPY(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1664, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1697, self, varargin{:});
     end
     function varargout = getInternalStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1665, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1698, self, varargin{:});
     end
     function varargout = getInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1666, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1699, self, varargin{:});
     end
     function varargout = getDefaultInternalInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1667, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1700, self, varargin{:});
     end
     function varargout = setInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1668, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1701, self, varargin{:});
     end
     function varargout = setInternalStateInitialOrientation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1669, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1702, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1670, self);
+        iDynTreeMEX(1703, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
index c59340d0177..ad3303e74c0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
@@ -7,50 +7,50 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1639, self);
+        varargout{1} = iDynTreeMEX(1672, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1640, self, varargin{1});
+        iDynTreeMEX(1673, self, varargin{1});
       end
     end
     function varargout = kp(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1641, self);
+        varargout{1} = iDynTreeMEX(1674, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1642, self, varargin{1});
+        iDynTreeMEX(1675, self, varargin{1});
       end
     end
     function varargout = ki(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1643, self);
+        varargout{1} = iDynTreeMEX(1676, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1644, self, varargin{1});
+        iDynTreeMEX(1677, self, varargin{1});
       end
     end
     function varargout = use_magnetometer_measurements(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1645, self);
+        varargout{1} = iDynTreeMEX(1678, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1646, self, varargin{1});
+        iDynTreeMEX(1679, self, varargin{1});
       end
     end
     function varargout = confidence_magnetometer_measurements(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1647, self);
+        varargout{1} = iDynTreeMEX(1680, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1648, self, varargin{1});
+        iDynTreeMEX(1681, self, varargin{1});
       end
     end
     function self = AttitudeMahonyFilterParameters(varargin)
@@ -59,14 +59,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1649, varargin{:});
+        tmp = iDynTreeMEX(1682, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1650, self);
+        iDynTreeMEX(1683, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
index d229b8d6880..0072f02c5c5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
@@ -11,74 +11,74 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1724, varargin{:});
+        tmp = iDynTreeMEX(1749, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = getParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1725, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1750, self, varargin{:});
     end
     function varargout = setParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1726, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1751, self, varargin{:});
     end
     function varargout = setGravityDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1727, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1752, self, varargin{:});
     end
     function varargout = setTimeStepInSeconds(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1728, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1753, self, varargin{:});
     end
     function varargout = setBiasCorrelationTimeFactor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1729, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1754, self, varargin{:});
     end
     function varargout = useMagnetometerMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1730, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1755, self, varargin{:});
     end
     function varargout = setMeasurementNoiseVariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1731, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1756, self, varargin{:});
     end
     function varargout = setSystemNoiseVariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1732, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1757, self, varargin{:});
     end
     function varargout = setInitialStateCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1733, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1758, self, varargin{:});
     end
     function varargout = initializeFilter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1734, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1759, self, varargin{:});
     end
     function varargout = updateFilterWithMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1735, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1760, self, varargin{:});
     end
     function varargout = propagateStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1736, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1761, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1737, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1762, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsQuaternion(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1738, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1763, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRPY(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1739, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1764, self, varargin{:});
     end
     function varargout = getInternalStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1740, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1765, self, varargin{:});
     end
     function varargout = getInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1741, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1766, self, varargin{:});
     end
     function varargout = getDefaultInternalInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1742, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1767, self, varargin{:});
     end
     function varargout = setInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1743, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1768, self, varargin{:});
     end
     function varargout = setInternalStateInitialOrientation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1744, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1769, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1745, self);
+        iDynTreeMEX(1770, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
index 18752c94df2..fe645eab69a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
@@ -7,100 +7,100 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1702, self);
+        varargout{1} = iDynTreeMEX(1727, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1703, self, varargin{1});
+        iDynTreeMEX(1728, self, varargin{1});
       end
     end
     function varargout = bias_correlation_time_factor(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1704, self);
+        varargout{1} = iDynTreeMEX(1729, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1705, self, varargin{1});
+        iDynTreeMEX(1730, self, varargin{1});
       end
     end
     function varargout = accelerometer_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1706, self);
+        varargout{1} = iDynTreeMEX(1731, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1707, self, varargin{1});
+        iDynTreeMEX(1732, self, varargin{1});
       end
     end
     function varargout = magnetometer_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1708, self);
+        varargout{1} = iDynTreeMEX(1733, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1709, self, varargin{1});
+        iDynTreeMEX(1734, self, varargin{1});
       end
     end
     function varargout = gyroscope_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1710, self);
+        varargout{1} = iDynTreeMEX(1735, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1711, self, varargin{1});
+        iDynTreeMEX(1736, self, varargin{1});
       end
     end
     function varargout = gyro_bias_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1712, self);
+        varargout{1} = iDynTreeMEX(1737, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1713, self, varargin{1});
+        iDynTreeMEX(1738, self, varargin{1});
       end
     end
     function varargout = initial_orientation_error_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1714, self);
+        varargout{1} = iDynTreeMEX(1739, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1715, self, varargin{1});
+        iDynTreeMEX(1740, self, varargin{1});
       end
     end
     function varargout = initial_ang_vel_error_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1716, self);
+        varargout{1} = iDynTreeMEX(1741, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1717, self, varargin{1});
+        iDynTreeMEX(1742, self, varargin{1});
       end
     end
     function varargout = initial_gyro_bias_error_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1718, self);
+        varargout{1} = iDynTreeMEX(1743, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1719, self, varargin{1});
+        iDynTreeMEX(1744, self, varargin{1});
       end
     end
     function varargout = use_magnetometer_measurements(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1720, self);
+        varargout{1} = iDynTreeMEX(1745, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1721, self, varargin{1});
+        iDynTreeMEX(1746, self, varargin{1});
       end
     end
     function self = AttitudeQuaternionEKFParameters(varargin)
@@ -109,14 +109,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1722, varargin{:});
+        tmp = iDynTreeMEX(1747, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1723, self);
+        iDynTreeMEX(1748, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
index 0fb3cc533e0..eeae97d5cfa 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
@@ -7,37 +7,37 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1570, self);
+        varargout{1} = iDynTreeMEX(1595, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1571, self, varargin{1});
+        iDynTreeMEX(1596, self, varargin{1});
       end
     end
     function varargout = id(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1572, self);
+        varargout{1} = iDynTreeMEX(1597, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1573, self, varargin{1});
+        iDynTreeMEX(1598, self, varargin{1});
       end
     end
     function varargout = range(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1574, self);
+        varargout{1} = iDynTreeMEX(1599, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1575, self, varargin{1});
+        iDynTreeMEX(1600, self, varargin{1});
       end
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1576, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1601, self, varargin{:});
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1577, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1602, self, varargin{:});
     end
     function self = BerdyDynamicVariable(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -45,14 +45,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1578, varargin{:});
+        tmp = iDynTreeMEX(1603, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1579, self);
+        iDynTreeMEX(1604, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m b/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
index 5259efa69fe..bb092a248fc 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
@@ -9,104 +9,104 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1580, varargin{:});
+        tmp = iDynTreeMEX(1605, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = dynamicTraversal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1581, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1606, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1582, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1607, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1583, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1608, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1584, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1609, self, varargin{:});
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1585, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1610, self, varargin{:});
     end
     function varargout = getOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1586, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1611, self, varargin{:});
     end
     function varargout = getNrOfDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1587, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1612, self, varargin{:});
     end
     function varargout = getNrOfDynamicEquations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1588, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1613, self, varargin{:});
     end
     function varargout = getNrOfSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1589, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1614, self, varargin{:});
     end
     function varargout = resizeAndZeroBerdyMatrices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1590, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1615, self, varargin{:});
     end
     function varargout = getBerdyMatrices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1591, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1616, self, varargin{:});
     end
     function varargout = getSensorsOrdering(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1592, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1617, self, varargin{:});
     end
     function varargout = getRangeSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1593, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1618, self, varargin{:});
     end
     function varargout = getRangeDOFSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1594, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1619, self, varargin{:});
     end
     function varargout = getRangeJointSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1595, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1620, self, varargin{:});
     end
     function varargout = getRangeLinkSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1596, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1621, self, varargin{:});
     end
     function varargout = getRangeLinkVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1597, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1622, self, varargin{:});
     end
     function varargout = getRangeJointVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1598, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1623, self, varargin{:});
     end
     function varargout = getRangeDOFVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1599, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1624, self, varargin{:});
     end
     function varargout = getDynamicVariablesOrdering(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1600, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1625, self, varargin{:});
     end
     function varargout = serializeDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1601, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1626, self, varargin{:});
     end
     function varargout = serializeSensorVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1602, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1627, self, varargin{:});
     end
     function varargout = serializeDynamicVariablesComputedFromFixedBaseRNEA(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1603, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1628, self, varargin{:});
     end
     function varargout = extractJointTorquesFromDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1604, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1629, self, varargin{:});
     end
     function varargout = extractLinkNetExternalWrenchesFromDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1605, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1630, self, varargin{:});
     end
     function varargout = updateKinematicsFromFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1606, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1631, self, varargin{:});
     end
     function varargout = updateKinematicsFromFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1607, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1632, self, varargin{:});
     end
     function varargout = updateKinematicsFromTraversalFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1608, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1633, self, varargin{:});
     end
     function varargout = setNetExternalWrenchMeasurementFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1609, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1634, self, varargin{:});
     end
     function varargout = getNetExternalWrenchMeasurementFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1610, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1635, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1611, self);
+        iDynTreeMEX(1636, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m b/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
index 9bdc6e1c612..3eb7bff8fbd 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
@@ -9,7 +9,7 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1541, varargin{:});
+        tmp = iDynTreeMEX(1566, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
@@ -18,88 +18,88 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1542, self);
+        varargout{1} = iDynTreeMEX(1567, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1543, self, varargin{1});
+        iDynTreeMEX(1568, self, varargin{1});
       end
     end
     function varargout = includeAllNetExternalWrenchesAsDynamicVariables(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1544, self);
+        varargout{1} = iDynTreeMEX(1569, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1545, self, varargin{1});
+        iDynTreeMEX(1570, self, varargin{1});
       end
     end
     function varargout = includeAllJointAccelerationsAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1546, self);
+        varargout{1} = iDynTreeMEX(1571, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1547, self, varargin{1});
+        iDynTreeMEX(1572, self, varargin{1});
       end
     end
     function varargout = includeAllJointTorquesAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1548, self);
+        varargout{1} = iDynTreeMEX(1573, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1549, self, varargin{1});
+        iDynTreeMEX(1574, self, varargin{1});
       end
     end
     function varargout = includeAllNetExternalWrenchesAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1550, self);
+        varargout{1} = iDynTreeMEX(1575, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1551, self, varargin{1});
+        iDynTreeMEX(1576, self, varargin{1});
       end
     end
     function varargout = includeFixedBaseExternalWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1552, self);
+        varargout{1} = iDynTreeMEX(1577, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1553, self, varargin{1});
+        iDynTreeMEX(1578, self, varargin{1});
       end
     end
     function varargout = jointOnWhichTheInternalWrenchIsMeasured(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1554, self);
+        varargout{1} = iDynTreeMEX(1579, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1555, self, varargin{1});
+        iDynTreeMEX(1580, self, varargin{1});
       end
     end
     function varargout = baseLink(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1556, self);
+        varargout{1} = iDynTreeMEX(1581, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1557, self, varargin{1});
+        iDynTreeMEX(1582, self, varargin{1});
       end
     end
     function varargout = checkConsistency(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1558, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1583, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1559, self);
+        iDynTreeMEX(1584, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m b/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
index ef16db95a0b..3967108b759 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
@@ -7,37 +7,37 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1560, self);
+        varargout{1} = iDynTreeMEX(1585, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1561, self, varargin{1});
+        iDynTreeMEX(1586, self, varargin{1});
       end
     end
     function varargout = id(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1562, self);
+        varargout{1} = iDynTreeMEX(1587, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1563, self, varargin{1});
+        iDynTreeMEX(1588, self, varargin{1});
       end
     end
     function varargout = range(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1564, self);
+        varargout{1} = iDynTreeMEX(1589, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1565, self, varargin{1});
+        iDynTreeMEX(1590, self, varargin{1});
       end
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1566, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1591, self, varargin{:});
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1567, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1592, self, varargin{:});
     end
     function self = BerdySensor(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -45,14 +45,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1568, varargin{:});
+        tmp = iDynTreeMEX(1593, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1569, self);
+        iDynTreeMEX(1594, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m b/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
index f7ba5878a7c..44139b674ee 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
@@ -9,58 +9,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1612, varargin{:});
+        tmp = iDynTreeMEX(1637, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1613, self);
+        iDynTreeMEX(1638, self);
         self.SwigClear();
       end
     end
     function varargout = setDynamicsConstraintsPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1614, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1639, self, varargin{:});
     end
     function varargout = setDynamicsRegularizationPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1615, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1640, self, varargin{:});
     end
     function varargout = setDynamicsRegularizationPriorExpectedValue(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1616, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1641, self, varargin{:});
     end
     function varargout = setMeasurementsPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1617, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1642, self, varargin{:});
     end
     function varargout = dynamicsConstraintsPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1618, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1643, self, varargin{:});
     end
     function varargout = dynamicsRegularizationPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1619, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1644, self, varargin{:});
     end
     function varargout = dynamicsRegularizationPriorExpectedValue(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1620, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1645, self, varargin{:});
     end
     function varargout = measurementsPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1621, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1646, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1622, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1647, self, varargin{:});
     end
     function varargout = initialize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1623, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1648, self, varargin{:});
     end
     function varargout = updateEstimateInformationFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1624, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1649, self, varargin{:});
     end
     function varargout = updateEstimateInformationFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1625, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1650, self, varargin{:});
     end
     function varargout = doEstimate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1626, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1651, self, varargin{:});
     end
     function varargout = getLastEstimate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1627, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1652, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/Box.m b/bindings/matlab/autogenerated/+iDynTree/Box.m
index 7bb8d955905..822049af1b6 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Box.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Box.m
@@ -2,41 +2,41 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1110, self);
+        iDynTreeMEX(1116, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1111, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1117, self, varargin{:});
     end
     function varargout = x(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1112, self);
+        varargout{1} = iDynTreeMEX(1118, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1113, self, varargin{1});
+        iDynTreeMEX(1119, self, varargin{1});
       end
     end
     function varargout = y(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1114, self);
+        varargout{1} = iDynTreeMEX(1120, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1115, self, varargin{1});
+        iDynTreeMEX(1121, self, varargin{1});
       end
     end
     function varargout = z(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1116, self);
+        varargout{1} = iDynTreeMEX(1122, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1117, self, varargin{1});
+        iDynTreeMEX(1123, self, varargin{1});
       end
     end
     function self = Box(varargin)
@@ -46,7 +46,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1118, varargin{:});
+        tmp = iDynTreeMEX(1124, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ColorViz.m b/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
index cb6bab3b622..aa221b1ae43 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1855, self);
+        varargout{1} = iDynTreeMEX(1881, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1856, self, varargin{1});
+        iDynTreeMEX(1882, self, varargin{1});
       end
     end
     function varargout = g(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1857, self);
+        varargout{1} = iDynTreeMEX(1883, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1858, self, varargin{1});
+        iDynTreeMEX(1884, self, varargin{1});
       end
     end
     function varargout = b(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1859, self);
+        varargout{1} = iDynTreeMEX(1885, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1860, self, varargin{1});
+        iDynTreeMEX(1886, self, varargin{1});
       end
     end
     function varargout = a(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1861, self);
+        varargout{1} = iDynTreeMEX(1887, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1862, self, varargin{1});
+        iDynTreeMEX(1888, self, varargin{1});
       end
     end
     function self = ColorViz(varargin)
@@ -49,14 +49,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1863, varargin{:});
+        tmp = iDynTreeMEX(1889, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1864, self);
+        iDynTreeMEX(1890, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m b/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m
index 0ea796ae149..63ffcd6eb3b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m
+++ b/bindings/matlab/autogenerated/+iDynTree/CompositeRigidBodyAlgorithm.m
@@ -1,3 +1,3 @@
 function varargout = CompositeRigidBodyAlgorithm(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1261, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1269, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m
index 9e14f6d4fed..085a74d111b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentum.m
@@ -1,3 +1,3 @@
 function varargout = ComputeLinearAndAngularMomentum(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1258, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1266, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m
index 72efc51b0d5..54cf6286e53 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ComputeLinearAndAngularMomentumDerivativeBias.m
@@ -1,3 +1,3 @@
 function varargout = ComputeLinearAndAngularMomentumDerivativeBias(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1259, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1267, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m b/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m
index 5639709d646..5a097ec47e0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ContactWrench.m
@@ -4,13 +4,13 @@
       this = iDynTreeMEX(3, self);
     end
     function varargout = contactId(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1238, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1246, self, varargin{:});
     end
     function varargout = contactPoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1239, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1247, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1240, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1248, self, varargin{:});
     end
     function self = ContactWrench(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -18,14 +18,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1241, varargin{:});
+        tmp = iDynTreeMEX(1249, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1242, self);
+        iDynTreeMEX(1250, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Cylinder.m b/bindings/matlab/autogenerated/+iDynTree/Cylinder.m
index e8b26d1a381..13eb271af41 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Cylinder.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Cylinder.m
@@ -2,31 +2,31 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1119, self);
+        iDynTreeMEX(1125, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1120, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1126, self, varargin{:});
     end
     function varargout = length(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1121, self);
+        varargout{1} = iDynTreeMEX(1127, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1122, self, varargin{1});
+        iDynTreeMEX(1128, self, varargin{1});
       end
     end
     function varargout = radius(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1123, self);
+        varargout{1} = iDynTreeMEX(1129, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1124, self, varargin{1});
+        iDynTreeMEX(1130, self, varargin{1});
       end
     end
     function self = Cylinder(varargin)
@@ -36,7 +36,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1125, varargin{:});
+        tmp = iDynTreeMEX(1131, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m
index 539e203eb47..2d487487084 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialForceArray.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1193, varargin{:});
+        tmp = iDynTreeMEX(1201, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1194, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1202, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1195, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1203, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1196, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1204, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1197, self);
+        iDynTreeMEX(1205, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m
index d0e1a7055f0..d8776e7ea40 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOFSpatialMotionArray.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1198, varargin{:});
+        tmp = iDynTreeMEX(1206, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1199, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1207, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1200, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1208, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1201, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1209, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1202, self);
+        iDynTreeMEX(1210, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m
index b3d5b249d7f..281a743d380 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(847);
+    varargout{1} = iDynTreeMEX(851);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(848,varargin{1});
+    iDynTreeMEX(852,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m
index ecc01cf9c48..0c103f7f67c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DOF_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(849);
+    varargout{1} = iDynTreeMEX(853);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(850,varargin{1});
+    iDynTreeMEX(854,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m b/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
index 0793cfb0e42..09b666d94c1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
@@ -4,65 +4,65 @@
       this = iDynTreeMEX(3, self);
     end
     function varargout = ekf_f(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1671, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1704, self, varargin{:});
     end
     function varargout = ekf_h(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1672, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1705, self, varargin{:});
     end
     function varargout = ekfComputeJacobianF(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1673, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1706, self, varargin{:});
     end
     function varargout = ekfComputeJacobianH(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1674, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1707, self, varargin{:});
     end
     function varargout = ekfPredict(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1675, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1708, self, varargin{:});
     end
     function varargout = ekfUpdate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1676, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1709, self, varargin{:});
     end
     function varargout = ekfInit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1677, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1710, self, varargin{:});
     end
     function varargout = ekfReset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1678, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1711, self, varargin{:});
     end
     function varargout = ekfSetMeasurementVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1679, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1712, self, varargin{:});
     end
     function varargout = ekfSetInputVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1680, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1713, self, varargin{:});
     end
     function varargout = ekfSetInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1681, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1714, self, varargin{:});
     end
     function varargout = ekfSetStateCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1682, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1715, self, varargin{:});
     end
     function varargout = ekfSetSystemNoiseCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1683, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1716, self, varargin{:});
     end
     function varargout = ekfSetMeasurementNoiseCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1684, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1717, self, varargin{:});
     end
     function varargout = ekfSetStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1685, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1718, self, varargin{:});
     end
     function varargout = ekfSetInputSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1686, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1719, self, varargin{:});
     end
     function varargout = ekfSetOutputSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1687, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1720, self, varargin{:});
     end
     function varargout = ekfGetStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1688, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1721, self, varargin{:});
     end
     function varargout = ekfGetStateCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1689, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1722, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1690, self);
+        iDynTreeMEX(1723, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m b/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m
index f6ae3cdc1bf..f3619e8c873 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicSpan.m
@@ -9,78 +9,78 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(818, varargin{:});
+        tmp = iDynTreeMEX(822, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(819, self);
+        iDynTreeMEX(823, self);
         self.SwigClear();
       end
     end
     function varargout = first(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(820, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(824, self, varargin{:});
     end
     function varargout = last(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(821, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(825, self, varargin{:});
     end
     function varargout = subspan(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(822, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(826, self, varargin{:});
     end
     function varargout = size(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(823, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(827, self, varargin{:});
     end
     function varargout = size_bytes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(824, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(828, self, varargin{:});
     end
     function varargout = empty(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(825, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(829, self, varargin{:});
     end
     function varargout = brace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(826, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(830, self, varargin{:});
     end
     function varargout = getVal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(827, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(831, self, varargin{:});
     end
     function varargout = setVal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(828, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(832, self, varargin{:});
     end
     function varargout = at(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(829, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(833, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(830, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(834, self, varargin{:});
     end
     function varargout = begin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(831, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(835, self, varargin{:});
     end
     function varargout = end(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(832, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(836, self, varargin{:});
     end
     function varargout = cbegin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(833, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(837, self, varargin{:});
     end
     function varargout = cend(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(834, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(838, self, varargin{:});
     end
     function varargout = rbegin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(835, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(839, self, varargin{:});
     end
     function varargout = rend(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(836, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(840, self, varargin{:});
     end
     function varargout = crbegin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(837, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(841, self, varargin{:});
     end
     function varargout = crend(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(838, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(842, self, varargin{:});
     end
   end
   methods(Static)
     function v = extent()
-      v = iDynTreeMEX(817);
+      v = iDynTreeMEX(821);
     end
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
index 1b1f4eb718d..19b99773f05 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
@@ -9,118 +9,118 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1945, varargin{:});
+        tmp = iDynTreeMEX(1971, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1946, self);
+        iDynTreeMEX(1972, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1947, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1973, self, varargin{:});
     end
     function varargout = loadRobotModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1948, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1974, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1949, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1975, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1950, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1976, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1951, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1977, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1952, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1978, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1953, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1979, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1954, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1980, self, varargin{:});
     end
     function varargout = getFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1955, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1981, self, varargin{:});
     end
     function varargout = setFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1956, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1982, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1957, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1983, self, varargin{:});
     end
     function varargout = getWorldBaseTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1958, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1984, self, varargin{:});
     end
     function varargout = getBaseTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1959, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1985, self, varargin{:});
     end
     function varargout = getJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1960, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1986, self, varargin{:});
     end
     function varargout = getJointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1961, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1987, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1962, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1988, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1963, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1989, self, varargin{:});
     end
     function varargout = getWorldTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1964, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1990, self, varargin{:});
     end
     function varargout = getRelativeTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1965, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1991, self, varargin{:});
     end
     function varargout = getFrameTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1966, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1992, self, varargin{:});
     end
     function varargout = getFrameTwistInWorldOrient(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1967, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1993, self, varargin{:});
     end
     function varargout = getFrameProperSpatialAcceleration(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1968, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1994, self, varargin{:});
     end
     function varargout = getLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1969, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1995, self, varargin{:});
     end
     function varargout = getLinkInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1970, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1996, self, varargin{:});
     end
     function varargout = getJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1971, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1997, self, varargin{:});
     end
     function varargout = getJointName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1972, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1998, self, varargin{:});
     end
     function varargout = getJointLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1973, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1999, self, varargin{:});
     end
     function varargout = inverseDynamics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1974, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2000, self, varargin{:});
     end
     function varargout = getFreeFloatingMassMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1975, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2001, self, varargin{:});
     end
     function varargout = getFrameJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1976, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2002, self, varargin{:});
     end
     function varargout = getDynamicsRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1977, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2003, self, varargin{:});
     end
     function varargout = getModelDynamicsParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1978, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2004, self, varargin{:});
     end
     function varargout = getCenterOfMass(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1979, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2005, self, varargin{:});
     end
     function varargout = getCenterOfMassJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1980, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2006, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
index 15d69d9051b..f51a712ce43 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
@@ -9,100 +9,100 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1766, varargin{:});
+        tmp = iDynTreeMEX(1792, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1767, self);
+        iDynTreeMEX(1793, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotAndSensorsModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1768, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1794, self, varargin{:});
     end
     function varargout = loadRobotAndSensorsModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1769, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1795, self, varargin{:});
     end
     function varargout = loadRegressorStructureFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1770, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1796, self, varargin{:});
     end
     function varargout = loadRegressorStructureFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1771, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1797, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1772, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1798, self, varargin{:});
     end
     function varargout = getNrOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1773, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1799, self, varargin{:});
     end
     function varargout = getNrOfOutputs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1774, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1800, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1775, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1801, self, varargin{:});
     end
     function varargout = getDescriptionOfParameter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1776, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1802, self, varargin{:});
     end
     function varargout = getDescriptionOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1777, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1803, self, varargin{:});
     end
     function varargout = getDescriptionOfOutput(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1778, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1804, self, varargin{:});
     end
     function varargout = getDescriptionOfOutputs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1779, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1805, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1780, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1806, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1781, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1807, self, varargin{:});
     end
     function varargout = getDescriptionOfLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1782, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1808, self, varargin{:});
     end
     function varargout = getDescriptionOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1783, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1809, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1784, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1810, self, varargin{:});
     end
     function varargout = getNrOfFakeLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1785, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1811, self, varargin{:});
     end
     function varargout = getBaseLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1786, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1812, self, varargin{:});
     end
     function varargout = getSensorsModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1787, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1813, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1788, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1814, self, varargin{:});
     end
     function varargout = getSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1789, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1815, self, varargin{:});
     end
     function varargout = setTorqueSensorMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1790, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1816, self, varargin{:});
     end
     function varargout = computeRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1791, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1817, self, varargin{:});
     end
     function varargout = getModelParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1792, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1818, self, varargin{:});
     end
     function varargout = computeFloatingBaseIdentifiableSubspace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1793, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1819, self, varargin{:});
     end
     function varargout = computeFixedBaseIdentifiableSubspace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1794, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1820, self, varargin{:});
     end
     function varargout = generate_random_regressors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1795, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1821, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
index 7eb0344f8de..b5b76705269 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1746, self);
+        varargout{1} = iDynTreeMEX(1772, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1747, self, varargin{1});
+        iDynTreeMEX(1773, self, varargin{1});
       end
     end
     function varargout = elemIndex(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1748, self);
+        varargout{1} = iDynTreeMEX(1774, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1749, self, varargin{1});
+        iDynTreeMEX(1775, self, varargin{1});
       end
     end
     function varargout = type(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1750, self);
+        varargout{1} = iDynTreeMEX(1776, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1751, self, varargin{1});
+        iDynTreeMEX(1777, self, varargin{1});
       end
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1752, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1778, self, varargin{:});
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1753, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1779, self, varargin{:});
     end
     function varargout = ne(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1754, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1780, self, varargin{:});
     end
     function self = DynamicsRegressorParameter(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -48,14 +48,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1755, varargin{:});
+        tmp = iDynTreeMEX(1781, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1756, self);
+        iDynTreeMEX(1782, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
index 8e166979883..23800572bdf 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
@@ -7,26 +7,26 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1757, self);
+        varargout{1} = iDynTreeMEX(1783, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1758, self, varargin{1});
+        iDynTreeMEX(1784, self, varargin{1});
       end
     end
     function varargout = getDescriptionOfParameter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1759, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1785, self, varargin{:});
     end
     function varargout = addParam(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1760, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1786, self, varargin{:});
     end
     function varargout = addList(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1761, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1787, self, varargin{:});
     end
     function varargout = findParam(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1762, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1788, self, varargin{:});
     end
     function varargout = getNrOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1763, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1789, self, varargin{:});
     end
     function self = DynamicsRegressorParametersList(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -34,14 +34,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1764, varargin{:});
+        tmp = iDynTreeMEX(1790, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1765, self);
+        iDynTreeMEX(1791, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m b/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
index 12eccf7781c..06e4f8cd882 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
@@ -9,52 +9,52 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1513, varargin{:});
+        tmp = iDynTreeMEX(1537, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1514, self);
+        iDynTreeMEX(1538, self);
         self.SwigClear();
       end
     end
     function varargout = setModelAndSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1515, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1539, self, varargin{:});
     end
     function varargout = loadModelAndSensorsFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1516, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1540, self, varargin{:});
     end
     function varargout = loadModelAndSensorsFromFileWithSpecifiedDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1517, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1541, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1518, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1542, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1519, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1543, self, varargin{:});
     end
     function varargout = submodels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1520, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1544, self, varargin{:});
     end
     function varargout = updateKinematicsFromFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1521, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1545, self, varargin{:});
     end
     function varargout = updateKinematicsFromFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1522, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1546, self, varargin{:});
     end
     function varargout = computeExpectedFTSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1523, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1547, self, varargin{:});
     end
     function varargout = estimateExtWrenchesAndJointTorques(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1524, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1548, self, varargin{:});
     end
     function varargout = checkThatTheModelIsStill(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1525, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1549, self, varargin{:});
     end
     function varargout = estimateLinkNetWrenchesWithoutGravity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1526, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1550, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m b/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m
index dfc704e1ee7..75c46a6ae2e 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ExternalMesh.m
@@ -2,31 +2,31 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1126, self);
+        iDynTreeMEX(1132, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1127, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1133, self, varargin{:});
     end
     function varargout = filename(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1128, self);
+        varargout{1} = iDynTreeMEX(1134, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1129, self, varargin{1});
+        iDynTreeMEX(1135, self, varargin{1});
       end
     end
     function varargout = scale(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1130, self);
+        varargout{1} = iDynTreeMEX(1136, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1131, self, varargin{1});
+        iDynTreeMEX(1137, self, varargin{1});
       end
     end
     function self = ExternalMesh(varargin)
@@ -36,7 +36,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1132, varargin{:});
+        tmp = iDynTreeMEX(1138, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m
index 152663177d0..8430f933964 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(851);
+    varargout{1} = iDynTreeMEX(855);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(852,varargin{1});
+    iDynTreeMEX(856,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m
index 9e3c4d061af..43406ada8c8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FRAME_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(853);
+    varargout{1} = iDynTreeMEX(857);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(854,varargin{1});
+    iDynTreeMEX(858,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m b/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m
index 3fa0dbeecb0..aa429355358 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FixedJoint.m
@@ -7,103 +7,103 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(937, varargin{:});
+        tmp = iDynTreeMEX(941, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(938, self);
+        iDynTreeMEX(942, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(939, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(943, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(940, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(944, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(941, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(945, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(942, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(946, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(943, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(947, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(944, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(948, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(945, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(949, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(946, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(950, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(947, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(951, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(948, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(952, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(949, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(953, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(950, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(954, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(951, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(955, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(952, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(956, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(953, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(957, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(954, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(958, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(955, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(959, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(956, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(960, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(957, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(961, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(958, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(962, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(959, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(963, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(960, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(964, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(961, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(965, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(962, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(966, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(963, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(967, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(964, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(968, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(965, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(969, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(966, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(970, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(967, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(971, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m
index 274aed97076..5765e8c29dc 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1256, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1264, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m
index 0d00f00821d..11454d1e9ef 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardBiasAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardBiasAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1257, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1265, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m
index 4b315d47c82..1f40528cda5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardPosVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1254, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1262, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m
index 941959cc169..3086c1fcd6e 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardPosVelKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardPosVelKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1255, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1263, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m
index a617c6a2bb3..7c5d2d53c69 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardPositionKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardPositionKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1252, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1260, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m
index 81d3bb3a2ef..5e383c66195 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ForwardVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = ForwardVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1253, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1261, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m b/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m
index f66efc827bc..e90fa3c9d47 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FrameFreeFloatingJacobian.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1203, varargin{:});
+        tmp = iDynTreeMEX(1211, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1204, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1212, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1205, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1213, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1206, self);
+        iDynTreeMEX(1214, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m
index 60eb8fce85e..3a3ba2d8f99 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingAcc.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1232, varargin{:});
+        tmp = iDynTreeMEX(1240, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1233, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1241, self, varargin{:});
     end
     function varargout = baseAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1234, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1242, self, varargin{:});
     end
     function varargout = jointAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1235, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1243, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1236, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1244, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1237, self);
+        iDynTreeMEX(1245, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m
index 2505d7f8dc0..d7e5c046be5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingGeneralizedTorques.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1220, varargin{:});
+        tmp = iDynTreeMEX(1228, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1221, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1229, self, varargin{:});
     end
     function varargout = baseWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1222, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1230, self, varargin{:});
     end
     function varargout = jointTorques(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1223, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1231, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1224, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1232, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1225, self);
+        iDynTreeMEX(1233, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m
index 84ed283777b..30464aefe54 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingMassMatrix.m
@@ -7,17 +7,17 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1211, varargin{:});
+        tmp = iDynTreeMEX(1219, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1212, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1220, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1213, self);
+        iDynTreeMEX(1221, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m
index 53585e6b414..20bfb986cdb 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingPos.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1214, varargin{:});
+        tmp = iDynTreeMEX(1222, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1215, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1223, self, varargin{:});
     end
     function varargout = worldBasePos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1216, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1224, self, varargin{:});
     end
     function varargout = jointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1217, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1225, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1218, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1226, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1219, self);
+        iDynTreeMEX(1227, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m
index 273849c0cf4..e669eacf174 100644
--- a/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m
+++ b/bindings/matlab/autogenerated/+iDynTree/FreeFloatingVel.m
@@ -9,26 +9,26 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1226, varargin{:});
+        tmp = iDynTreeMEX(1234, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1227, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1235, self, varargin{:});
     end
     function varargout = baseVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1228, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1236, self, varargin{:});
     end
     function varargout = jointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1229, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1237, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1230, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1238, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1231, self);
+        iDynTreeMEX(1239, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m b/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m
index 13bb760b091..5357117f6a3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/GyroscopeSensor.m
@@ -7,58 +7,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1385, varargin{:});
+        tmp = iDynTreeMEX(1393, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1386, self);
+        iDynTreeMEX(1394, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1387, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1395, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1388, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1396, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1389, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1397, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1390, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1398, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1391, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1399, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1392, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1400, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1393, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1401, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1394, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1402, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1395, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1403, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1396, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1404, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1397, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1405, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1398, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1406, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1399, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1407, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1400, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1408, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m b/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
index d0334cbb391..9da9181c363 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
@@ -5,39 +5,39 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1628, self);
+        iDynTreeMEX(1661, self);
         self.SwigClear();
       end
     end
     function varargout = updateFilterWithMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1629, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1662, self, varargin{:});
     end
     function varargout = propagateStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1630, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1663, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1631, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1664, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsQuaternion(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1632, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1665, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRPY(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1633, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1666, self, varargin{:});
     end
     function varargout = getInternalStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1634, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1667, self, varargin{:});
     end
     function varargout = getInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1635, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1668, self, varargin{:});
     end
     function varargout = getDefaultInternalInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1636, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1669, self, varargin{:});
     end
     function varargout = setInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1637, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1670, self, varargin{:});
     end
     function varargout = setInternalStateInitialOrientation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1638, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1671, self, varargin{:});
     end
     function self = IAttitudeEstimator(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/ICamera.m b/bindings/matlab/autogenerated/+iDynTree/ICamera.m
index 5632ce6d9cc..82261667df3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ICamera.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ICamera.m
@@ -5,18 +5,18 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1851, self);
+        iDynTreeMEX(1877, self);
         self.SwigClear();
       end
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1852, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1878, self, varargin{:});
     end
     function varargout = setTarget(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1853, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1879, self, varargin{:});
     end
     function varargout = setUpVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1854, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1880, self, varargin{:});
     end
     function self = ICamera(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m b/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
index 56e883a12eb..3aba39646ae 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
@@ -5,33 +5,33 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1879, self);
+        iDynTreeMEX(1905, self);
         self.SwigClear();
       end
     end
     function varargout = getElements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1880, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1906, self, varargin{:});
     end
     function varargout = setElementVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1881, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1907, self, varargin{:});
     end
     function varargout = setBackgroundColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1882, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1908, self, varargin{:});
     end
     function varargout = setAmbientLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1883, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1909, self, varargin{:});
     end
     function varargout = getLights(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1884, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1910, self, varargin{:});
     end
     function varargout = addLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1885, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1911, self, varargin{:});
     end
     function varargout = lightViz(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1886, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1912, self, varargin{:});
     end
     function varargout = removeLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1887, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1913, self, varargin{:});
     end
     function self = IEnvironment(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
index f3ffecfc2a6..4c6888b65cc 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
@@ -5,30 +5,30 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1888, self);
+        iDynTreeMEX(1914, self);
         self.SwigClear();
       end
     end
     function varargout = setJetsFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1889, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1915, self, varargin{:});
     end
     function varargout = getNrOfJets(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1890, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1916, self, varargin{:});
     end
     function varargout = getJetDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1891, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1917, self, varargin{:});
     end
     function varargout = setJetDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1892, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1918, self, varargin{:});
     end
     function varargout = setJetColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1893, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1919, self, varargin{:});
     end
     function varargout = setJetsDimensions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1894, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1920, self, varargin{:});
     end
     function varargout = setJetsIntensity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1895, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1921, self, varargin{:});
     end
     function self = IJetsVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IJoint.m b/bindings/matlab/autogenerated/+iDynTree/IJoint.m
index c1135c53484..4cf97979807 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IJoint.m
@@ -5,108 +5,108 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(903, self);
+        iDynTreeMEX(907, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(904, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(908, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(905, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(909, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(906, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(910, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(907, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(911, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(908, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(912, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(909, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(913, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(910, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(914, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(911, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(915, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(912, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(916, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(913, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(917, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(914, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(918, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(915, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(919, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(916, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(920, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(917, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(921, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(918, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(922, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(919, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(923, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(920, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(924, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(921, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(925, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(922, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(926, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(923, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(927, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(924, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(928, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(925, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(929, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(926, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(930, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(927, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(931, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(928, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(932, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(929, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(933, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(930, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(934, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(931, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(935, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(932, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(936, self, varargin{:});
     end
     function varargout = isRevoluteJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(933, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(937, self, varargin{:});
     end
     function varargout = isFixedJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(934, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(938, self, varargin{:});
     end
     function varargout = asRevoluteJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(935, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(939, self, varargin{:});
     end
     function varargout = asFixedJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(936, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(940, self, varargin{:});
     end
     function self = IJoint(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/ILight.m b/bindings/matlab/autogenerated/+iDynTree/ILight.m
index e3d8fd2bb51..3746d5f21e7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ILight.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ILight.m
@@ -5,48 +5,48 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1865, self);
+        iDynTreeMEX(1891, self);
         self.SwigClear();
       end
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1866, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1892, self, varargin{:});
     end
     function varargout = setType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1867, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1893, self, varargin{:});
     end
     function varargout = getType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1868, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1894, self, varargin{:});
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1869, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1895, self, varargin{:});
     end
     function varargout = getPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1870, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1896, self, varargin{:});
     end
     function varargout = setDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1871, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1897, self, varargin{:});
     end
     function varargout = getDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1872, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1898, self, varargin{:});
     end
     function varargout = setAmbientColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1873, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1899, self, varargin{:});
     end
     function varargout = getAmbientColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1874, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1900, self, varargin{:});
     end
     function varargout = setSpecularColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1875, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1901, self, varargin{:});
     end
     function varargout = getSpecularColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1876, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1902, self, varargin{:});
     end
     function varargout = setDiffuseColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1877, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1903, self, varargin{:});
     end
     function varargout = getDiffuseColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1878, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1904, self, varargin{:});
     end
     function self = ILight(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
index 4763aa4d64b..aa05b38a006 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
@@ -5,57 +5,57 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1903, self);
+        iDynTreeMEX(1929, self);
         self.SwigClear();
       end
     end
     function varargout = setPositions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1904, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1930, self, varargin{:});
     end
     function varargout = setLinkPositions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1905, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1931, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1906, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1932, self, varargin{:});
     end
     function varargout = getInstanceName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1907, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1933, self, varargin{:});
     end
     function varargout = setModelVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1908, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1934, self, varargin{:});
     end
     function varargout = setModelColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1909, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1935, self, varargin{:});
     end
     function varargout = resetModelColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1910, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1936, self, varargin{:});
     end
     function varargout = setLinkColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1911, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1937, self, varargin{:});
     end
     function varargout = resetLinkColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1912, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1938, self, varargin{:});
     end
     function varargout = getLinkNames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1913, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1939, self, varargin{:});
     end
     function varargout = setLinkVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1914, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1940, self, varargin{:});
     end
     function varargout = getFeatures(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1915, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1941, self, varargin{:});
     end
     function varargout = setFeatureVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1916, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1942, self, varargin{:});
     end
     function varargout = jets(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1917, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1943, self, varargin{:});
     end
     function varargout = getWorldModelTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1918, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1944, self, varargin{:});
     end
     function varargout = getWorldLinkTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1919, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1945, self, varargin{:});
     end
     function self = IModelVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
index 9660f6a0265..10d0524834f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
@@ -5,27 +5,27 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1896, self);
+        iDynTreeMEX(1922, self);
         self.SwigClear();
       end
     end
     function varargout = addVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1897, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1923, self, varargin{:});
     end
     function varargout = getNrOfVectors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1898, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1924, self, varargin{:});
     end
     function varargout = getVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1899, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1925, self, varargin{:});
     end
     function varargout = updateVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1900, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1926, self, varargin{:});
     end
     function varargout = setVectorColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1901, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1927, self, varargin{:});
     end
     function varargout = setVectorsAspect(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1902, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1928, self, varargin{:});
     end
     function self = IVectorsVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m b/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m
index e22317fcdc1..0e0cab49c57 100644
--- a/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/InverseDynamicsInertialParametersRegressor.m
@@ -1,3 +1,3 @@
 function varargout = InverseDynamicsInertialParametersRegressor(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1285, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1293, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m
index 7184dc2f826..a83ff99a0fa 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(843);
+    varargout{1} = iDynTreeMEX(847);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(844,varargin{1});
+    iDynTreeMEX(848,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m
index bfe7bf00125..7dcd64ef457 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JOINT_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(845);
+    varargout{1} = iDynTreeMEX(849);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(846,varargin{1});
+    iDynTreeMEX(850,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m b/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m
index dfaf39752d7..60e9d6392cd 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JointDOFsDoubleArray.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1189, varargin{:});
+        tmp = iDynTreeMEX(1197, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1190, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1198, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1191, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1199, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1192, self);
+        iDynTreeMEX(1200, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m b/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m
index f057d806b29..1470a34a594 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JointPosDoubleArray.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1185, varargin{:});
+        tmp = iDynTreeMEX(1193, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1186, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1194, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1187, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1195, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1188, self);
+        iDynTreeMEX(1196, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/JointSensor.m b/bindings/matlab/autogenerated/+iDynTree/JointSensor.m
index beed170942a..61f78957450 100644
--- a/bindings/matlab/autogenerated/+iDynTree/JointSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/JointSensor.m
@@ -2,24 +2,24 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1299, self);
+        iDynTreeMEX(1307, self);
         self.SwigClear();
       end
     end
     function varargout = getParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1300, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1308, self, varargin{:});
     end
     function varargout = getParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1301, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1309, self, varargin{:});
     end
     function varargout = setParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1302, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1310, self, varargin{:});
     end
     function varargout = setParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1303, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1311, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1304, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1312, self, varargin{:});
     end
     function self = JointSensor(varargin)
       self@iDynTree.Sensor(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m b/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
index ef8e60ac8a1..767cbff3371 100644
--- a/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
+++ b/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
@@ -9,175 +9,175 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1796, varargin{:});
+        tmp = iDynTreeMEX(1822, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1797, self);
+        iDynTreeMEX(1823, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1798, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1824, self, varargin{:});
     end
     function varargout = loadRobotModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1799, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1825, self, varargin{:});
     end
     function varargout = loadRobotModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1800, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1826, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1801, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1827, self, varargin{:});
     end
     function varargout = setFrameVelocityRepresentation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1802, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1828, self, varargin{:});
     end
     function varargout = getFrameVelocityRepresentation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1803, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1829, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1804, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1830, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1805, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1831, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1806, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1832, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1807, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1833, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1808, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1834, self, varargin{:});
     end
     function varargout = getFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1809, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1835, self, varargin{:});
     end
     function varargout = setFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1810, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1836, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1811, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1837, self, varargin{:});
     end
     function varargout = getRobotModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1812, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1838, self, varargin{:});
     end
     function varargout = getRelativeJacobianSparsityPattern(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1813, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1839, self, varargin{:});
     end
     function varargout = getFrameFreeFloatingJacobianSparsityPattern(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1814, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1840, self, varargin{:});
     end
     function varargout = setJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1815, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1841, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1816, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1842, self, varargin{:});
     end
     function varargout = getRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1817, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1843, self, varargin{:});
     end
     function varargout = getWorldBaseTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1818, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1844, self, varargin{:});
     end
     function varargout = getBaseTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1819, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1845, self, varargin{:});
     end
     function varargout = getJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1820, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1846, self, varargin{:});
     end
     function varargout = getJointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1821, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1847, self, varargin{:});
     end
     function varargout = getModelVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1822, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1848, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1823, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1849, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1824, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1850, self, varargin{:});
     end
     function varargout = getWorldTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1825, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1851, self, varargin{:});
     end
     function varargout = getRelativeTransformExplicit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1826, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1852, self, varargin{:});
     end
     function varargout = getRelativeTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1827, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1853, self, varargin{:});
     end
     function varargout = getFrameVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1828, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1854, self, varargin{:});
     end
     function varargout = getFrameAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1829, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1855, self, varargin{:});
     end
     function varargout = getFrameFreeFloatingJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1830, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1856, self, varargin{:});
     end
     function varargout = getRelativeJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1831, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1857, self, varargin{:});
     end
     function varargout = getRelativeJacobianExplicit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1832, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1858, self, varargin{:});
     end
     function varargout = getFrameBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1833, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1859, self, varargin{:});
     end
     function varargout = getCenterOfMassPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1834, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1860, self, varargin{:});
     end
     function varargout = getCenterOfMassVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1835, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1861, self, varargin{:});
     end
     function varargout = getCenterOfMassJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1836, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1862, self, varargin{:});
     end
     function varargout = getCenterOfMassBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1837, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1863, self, varargin{:});
     end
     function varargout = getAverageVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1838, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1864, self, varargin{:});
     end
     function varargout = getAverageVelocityJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1839, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1865, self, varargin{:});
     end
     function varargout = getCentroidalAverageVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1840, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1866, self, varargin{:});
     end
     function varargout = getCentroidalAverageVelocityJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1841, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1867, self, varargin{:});
     end
     function varargout = getLinearAngularMomentum(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1842, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1868, self, varargin{:});
     end
     function varargout = getLinearAngularMomentumJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1843, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1869, self, varargin{:});
     end
     function varargout = getCentroidalTotalMomentum(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1844, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1870, self, varargin{:});
     end
     function varargout = getFreeFloatingMassMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1845, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1871, self, varargin{:});
     end
     function varargout = inverseDynamics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1846, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1872, self, varargin{:});
     end
     function varargout = generalizedBiasForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1847, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1873, self, varargin{:});
     end
     function varargout = generalizedGravityForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1848, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1874, self, varargin{:});
     end
     function varargout = generalizedExternalForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1849, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1875, self, varargin{:});
     end
     function varargout = inverseDynamicsInertialParametersRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1850, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1876, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m
index 1e46c9a1719..c558fe6fb15 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(839);
+    varargout{1} = iDynTreeMEX(843);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(840,varargin{1});
+    iDynTreeMEX(844,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m
index 498c1325753..e57380fb83b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LINK_INVALID_NAME.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(841);
+    varargout{1} = iDynTreeMEX(845);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(842,varargin{1});
+    iDynTreeMEX(846,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Link.m b/bindings/matlab/autogenerated/+iDynTree/Link.m
index 839cc5ff365..e87b9f144e4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Link.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Link.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(896, varargin{:});
+        tmp = iDynTreeMEX(900, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = inertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(897, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(901, self, varargin{:});
     end
     function varargout = setInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(898, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(902, self, varargin{:});
     end
     function varargout = getInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(899, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(903, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(900, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(904, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(901, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(905, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(902, self);
+        iDynTreeMEX(906, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m b/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m
index 9b9943aaf32..a45c173d9d8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkAccArray.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(889, varargin{:});
+        tmp = iDynTreeMEX(893, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(890, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(894, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(891, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(895, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(892, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(896, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(893, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(897, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(894, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(898, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(895, self);
+        iDynTreeMEX(899, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m b/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m
index b6b3800b939..198eb8f0a3d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkArticulatedBodyInertias.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(877, varargin{:});
+        tmp = iDynTreeMEX(881, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(878, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(882, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(879, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(883, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(880, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(884, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(881, self);
+        iDynTreeMEX(885, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m b/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m
index e5fb0a81664..0d626c09343 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkContactWrenches.m
@@ -9,35 +9,35 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1243, varargin{:});
+        tmp = iDynTreeMEX(1251, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1244, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1252, self, varargin{:});
     end
     function varargout = getNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1245, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1253, self, varargin{:});
     end
     function varargout = setNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1246, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1254, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1247, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1255, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1248, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1256, self, varargin{:});
     end
     function varargout = computeNetWrenches(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1249, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1257, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1250, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1258, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1251, self);
+        iDynTreeMEX(1259, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m b/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m
index 9d57ff07a8b..64251847b3c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkInertias.m
@@ -9,23 +9,23 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(872, varargin{:});
+        tmp = iDynTreeMEX(876, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(873, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(877, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(874, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(878, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(875, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(879, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(876, self);
+        iDynTreeMEX(880, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m b/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m
index 73977b8f48a..38d3fb5a1bf 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkPositions.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(857, varargin{:});
+        tmp = iDynTreeMEX(861, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(858, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(862, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(859, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(863, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(860, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(864, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(861, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(865, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(862, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(866, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(863, self);
+        iDynTreeMEX(867, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m b/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m
index dca85adaebc..2b29c8a273d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkSensor.m
@@ -2,30 +2,30 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1305, self);
+        iDynTreeMEX(1313, self);
         self.SwigClear();
       end
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1306, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1314, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1307, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1315, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1308, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1316, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1309, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1317, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1310, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1318, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1311, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1319, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1312, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1320, self, varargin{:});
     end
     function self = LinkSensor(varargin)
       self@iDynTree.Sensor(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m b/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
index dddb7bdec43..647fc6b8a3a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
@@ -9,41 +9,41 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1478, varargin{:});
+        tmp = iDynTreeMEX(1502, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = clear(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1479, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1503, self, varargin{:});
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1480, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1504, self, varargin{:});
     end
     function varargout = getNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1481, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1505, self, varargin{:});
     end
     function varargout = setNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1482, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1506, self, varargin{:});
     end
     function varargout = addNewContactForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1483, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1507, self, varargin{:});
     end
     function varargout = addNewContactInFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1484, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1508, self, varargin{:});
     end
     function varargout = addNewUnknownFullWrenchInFrameOrigin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1485, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1509, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1486, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1510, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1487, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1511, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1488, self);
+        iDynTreeMEX(1512, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m b/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m
index e42fb096166..2f45d7d34f7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkVelArray.m
@@ -9,29 +9,29 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(882, varargin{:});
+        tmp = iDynTreeMEX(886, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(883, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(887, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(884, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(888, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(885, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(889, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(886, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(890, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(887, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(891, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(888, self);
+        iDynTreeMEX(892, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m b/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m
index 99b05c49f47..0a87a0e3a9c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkWrenches.m
@@ -9,32 +9,32 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(864, varargin{:});
+        tmp = iDynTreeMEX(868, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(865, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(869, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(866, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(870, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(867, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(871, self, varargin{:});
     end
     function varargout = paren(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(868, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(872, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(869, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(873, self, varargin{:});
     end
     function varargout = zero(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(870, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(874, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(871, self);
+        iDynTreeMEX(875, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Model.m b/bindings/matlab/autogenerated/+iDynTree/Model.m
index b1d338ea032..03a63ada06a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Model.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Model.m
@@ -9,127 +9,133 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1146, varargin{:});
+        tmp = iDynTreeMEX(1152, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = copy(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1147, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1153, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1148, self);
+        iDynTreeMEX(1154, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1149, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1155, self, varargin{:});
     end
     function varargout = getLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1150, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1156, self, varargin{:});
     end
     function varargout = getLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1151, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1157, self, varargin{:});
     end
     function varargout = isValidLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1152, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1158, self, varargin{:});
     end
     function varargout = getLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1153, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1159, self, varargin{:});
     end
     function varargout = addLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1154, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1160, self, varargin{:});
     end
     function varargout = getNrOfJoints(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1155, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1161, self, varargin{:});
     end
     function varargout = getJointName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1156, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1162, self, varargin{:});
+    end
+    function varargout = getTotalMass(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1163, self, varargin{:});
     end
     function varargout = getJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1157, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1164, self, varargin{:});
     end
     function varargout = getJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1158, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1165, self, varargin{:});
     end
     function varargout = isValidJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1159, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1166, self, varargin{:});
     end
     function varargout = isLinkNameUsed(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1160, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1167, self, varargin{:});
     end
     function varargout = isJointNameUsed(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1161, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1168, self, varargin{:});
     end
     function varargout = isFrameNameUsed(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1162, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1169, self, varargin{:});
     end
     function varargout = addJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1163, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1170, self, varargin{:});
     end
     function varargout = addJointAndLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1164, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1171, self, varargin{:});
     end
     function varargout = insertLinkToExistingJointAndAddJointForDisplacedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1165, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1172, self, varargin{:});
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1166, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1173, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1167, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1174, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1168, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1175, self, varargin{:});
     end
     function varargout = addAdditionalFrameToLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1169, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1176, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1170, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1177, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1171, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1178, self, varargin{:});
     end
     function varargout = isValidFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1172, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1179, self, varargin{:});
     end
     function varargout = getFrameTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1173, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1180, self, varargin{:});
     end
     function varargout = getFrameLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1174, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1181, self, varargin{:});
+    end
+    function varargout = getLinkAdditionalFrames(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1182, self, varargin{:});
     end
     function varargout = getNrOfNeighbors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1175, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1183, self, varargin{:});
     end
     function varargout = getNeighbor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1176, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1184, self, varargin{:});
     end
     function varargout = setDefaultBaseLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1177, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1185, self, varargin{:});
     end
     function varargout = getDefaultBaseLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1178, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1186, self, varargin{:});
     end
     function varargout = computeFullTreeTraversal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1179, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1187, self, varargin{:});
     end
     function varargout = getInertialParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1180, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1188, self, varargin{:});
     end
     function varargout = updateInertialParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1181, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1189, self, varargin{:});
     end
     function varargout = visualSolidShapes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1182, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1190, self, varargin{:});
     end
     function varargout = collisionSolidShapes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1183, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1191, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1184, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1192, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelExporter.m b/bindings/matlab/autogenerated/+iDynTree/ModelExporter.m
new file mode 100644
index 00000000000..86b5bd23633
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelExporter.m
@@ -0,0 +1,50 @@
+classdef ModelExporter < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function self = ModelExporter(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1480, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1481, self);
+        self.SwigClear();
+      end
+    end
+    function varargout = exportingOptions(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1482, self, varargin{:});
+    end
+    function varargout = setExportingOptions(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1483, self, varargin{:});
+    end
+    function varargout = init(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1484, self, varargin{:});
+    end
+    function varargout = model(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1485, self, varargin{:});
+    end
+    function varargout = sensors(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1486, self, varargin{:});
+    end
+    function varargout = isValid(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1487, self, varargin{:});
+    end
+    function varargout = exportModelToString(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1488, self, varargin{:});
+    end
+    function varargout = exportModelToFile(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1489, self, varargin{:});
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelExporterOptions.m b/bindings/matlab/autogenerated/+iDynTree/ModelExporterOptions.m
new file mode 100644
index 00000000000..44fd65110e0
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelExporterOptions.m
@@ -0,0 +1,46 @@
+classdef ModelExporterOptions < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function varargout = baseLink(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1474, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1475, self, varargin{1});
+      end
+    end
+    function varargout = exportFirstBaseLinkAdditionalFrameAsFakeURDFBase(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1476, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1477, self, varargin{1});
+      end
+    end
+    function self = ModelExporterOptions(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1478, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1479, self);
+        self.SwigClear();
+      end
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m b/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m
index fcce286db30..b8285b14c77 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelLoader.m
@@ -9,46 +9,46 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1454, varargin{:});
+        tmp = iDynTreeMEX(1462, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1455, self);
+        iDynTreeMEX(1463, self);
         self.SwigClear();
       end
     end
     function varargout = parsingOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1456, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1464, self, varargin{:});
     end
     function varargout = setParsingOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1457, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1465, self, varargin{:});
     end
     function varargout = loadModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1458, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1466, self, varargin{:});
     end
     function varargout = loadModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1459, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1467, self, varargin{:});
     end
     function varargout = loadReducedModelFromFullModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1460, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1468, self, varargin{:});
     end
     function varargout = loadReducedModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1461, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1469, self, varargin{:});
     end
     function varargout = loadReducedModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1462, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1470, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1463, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1471, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1464, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1472, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1465, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1473, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m b/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m
index f877b37c941..77997c73cc8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelParserOptions.m
@@ -7,20 +7,20 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1448, self);
+        varargout{1} = iDynTreeMEX(1456, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1449, self, varargin{1});
+        iDynTreeMEX(1457, self, varargin{1});
       end
     end
     function varargout = originalFilename(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1450, self);
+        varargout{1} = iDynTreeMEX(1458, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1451, self, varargin{1});
+        iDynTreeMEX(1459, self, varargin{1});
       end
     end
     function self = ModelParserOptions(varargin)
@@ -29,14 +29,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1452, varargin{:});
+        tmp = iDynTreeMEX(1460, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1453, self);
+        iDynTreeMEX(1461, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m b/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m
index 7b32de45721..35a2d670436 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelSolidShapes.m
@@ -9,34 +9,34 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1133, varargin{:});
+        tmp = iDynTreeMEX(1139, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = clear(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1134, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1140, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1135, self);
+        iDynTreeMEX(1141, self);
         self.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1136, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1142, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1137, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1143, self, varargin{:});
     end
     function varargout = linkSolidShapes(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1138, self);
+        varargout{1} = iDynTreeMEX(1144, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1139, self, varargin{1});
+        iDynTreeMEX(1145, self, varargin{1});
       end
     end
   end
diff --git a/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m b/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m
index 167733c3ec1..3a39a5bf6de 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MomentumFreeFloatingJacobian.m
@@ -7,20 +7,20 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1207, varargin{:});
+        tmp = iDynTreeMEX(1215, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1208, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1216, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1209, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1217, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1210, self);
+        iDynTreeMEX(1218, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m
index c27613e7f90..521b88fdca7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl1.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(968, self);
+        iDynTreeMEX(972, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(969, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(973, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(970, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(974, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(971, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(975, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(972, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(976, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(973, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(977, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(974, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(978, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(975, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(979, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(976, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(980, self, varargin{:});
     end
     function self = MovableJointImpl1(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m
index 17182a7d7e7..449e76a4b67 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl2.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(977, self);
+        iDynTreeMEX(981, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(978, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(982, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(979, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(983, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(980, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(984, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(981, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(985, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(982, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(986, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(983, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(987, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(984, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(988, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(985, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(989, self, varargin{:});
     end
     function self = MovableJointImpl2(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m
index 07a8643c0f1..4fde31d7531 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl3.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(986, self);
+        iDynTreeMEX(990, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(987, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(991, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(988, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(992, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(989, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(993, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(990, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(994, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(991, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(995, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(992, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(996, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(993, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(997, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(994, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(998, self, varargin{:});
     end
     function self = MovableJointImpl3(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m
index d1f5829dcbe..f9dfc88cf72 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl4.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(995, self);
+        iDynTreeMEX(999, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(996, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1000, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(997, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1001, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(998, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1002, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(999, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1003, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1000, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1004, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1001, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1005, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1002, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1006, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1003, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1007, self, varargin{:});
     end
     function self = MovableJointImpl4(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m
index ea277a67688..84000fb283f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl5.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1004, self);
+        iDynTreeMEX(1008, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1005, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1009, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1006, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1010, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1007, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1011, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1008, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1012, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1009, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1013, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1010, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1014, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1011, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1015, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1012, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1016, self, varargin{:});
     end
     function self = MovableJointImpl5(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m
index 3c7e8f37c93..9502dbbb72c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m
+++ b/bindings/matlab/autogenerated/+iDynTree/MovableJointImpl6.m
@@ -2,33 +2,33 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1013, self);
+        iDynTreeMEX(1017, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfPosCoords(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1014, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1018, self, varargin{:});
     end
     function varargout = getNrOfDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1015, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1019, self, varargin{:});
     end
     function varargout = setIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1016, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1020, self, varargin{:});
     end
     function varargout = getIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1017, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1021, self, varargin{:});
     end
     function varargout = setPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1018, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1022, self, varargin{:});
     end
     function varargout = getPosCoordsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1019, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1023, self, varargin{:});
     end
     function varargout = setDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1020, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1024, self, varargin{:});
     end
     function varargout = getDOFsOffset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1021, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1025, self, varargin{:});
     end
     function self = MovableJointImpl6(varargin)
       self@iDynTree.IJoint(SwigRef.Null);
diff --git a/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m b/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m
index 4085e249c55..e0efabd6fb7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m
+++ b/bindings/matlab/autogenerated/+iDynTree/NR_OF_SENSOR_TYPES.m
@@ -1,3 +1,3 @@
 function v = NR_OF_SENSOR_TYPES()
-  v = iDynTreeMEX(1286);
+  v = iDynTreeMEX(1294);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Neighbor.m b/bindings/matlab/autogenerated/+iDynTree/Neighbor.m
index 2991e4f4982..ec274815e52 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Neighbor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Neighbor.m
@@ -7,20 +7,20 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1140, self);
+        varargout{1} = iDynTreeMEX(1146, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1141, self, varargin{1});
+        iDynTreeMEX(1147, self, varargin{1});
       end
     end
     function varargout = neighborJoint(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1142, self);
+        varargout{1} = iDynTreeMEX(1148, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1143, self, varargin{1});
+        iDynTreeMEX(1149, self, varargin{1});
       end
     end
     function self = Neighbor(varargin)
@@ -29,14 +29,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1144, varargin{:});
+        tmp = iDynTreeMEX(1150, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1145, self);
+        iDynTreeMEX(1151, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m b/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m
index cc05c34409f..8c890af1ea8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/PrismaticJoint.m
@@ -7,85 +7,85 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1047, varargin{:});
+        tmp = iDynTreeMEX(1051, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1048, self);
+        iDynTreeMEX(1052, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1049, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1053, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1050, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1054, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1051, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1055, self, varargin{:});
     end
     function varargout = setAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1052, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1056, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1053, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1057, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1054, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1058, self, varargin{:});
     end
     function varargout = getAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1055, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1059, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1056, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1060, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1057, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1061, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1058, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1062, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1059, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1063, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1060, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1064, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1061, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1065, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1062, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1066, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1063, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1067, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1064, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1068, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1065, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1069, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1066, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1070, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1067, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1071, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1068, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1072, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1069, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1073, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1070, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1074, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1071, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1075, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m b/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m
index 72fb05fb9d4..3866a4ba2ce 100644
--- a/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m
+++ b/bindings/matlab/autogenerated/+iDynTree/RNEADynamicPhase.m
@@ -1,3 +1,3 @@
 function varargout = RNEADynamicPhase(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1260, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1268, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m b/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m
index 39ed95cd3ac..1bc0c90d2a4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m
+++ b/bindings/matlab/autogenerated/+iDynTree/RevoluteJoint.m
@@ -7,85 +7,85 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1022, varargin{:});
+        tmp = iDynTreeMEX(1026, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1023, self);
+        iDynTreeMEX(1027, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1024, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1028, self, varargin{:});
     end
     function varargout = setAttachedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1025, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1029, self, varargin{:});
     end
     function varargout = setRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1026, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1030, self, varargin{:});
     end
     function varargout = setAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1027, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1031, self, varargin{:});
     end
     function varargout = getFirstAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1028, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1032, self, varargin{:});
     end
     function varargout = getSecondAttachedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1029, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1033, self, varargin{:});
     end
     function varargout = getAxis(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1030, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1034, self, varargin{:});
     end
     function varargout = getRestTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1031, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1035, self, varargin{:});
     end
     function varargout = getTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1032, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1036, self, varargin{:});
     end
     function varargout = getTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1033, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1037, self, varargin{:});
     end
     function varargout = getMotionSubspaceVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1034, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1038, self, varargin{:});
     end
     function varargout = computeChildPosVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1035, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1039, self, varargin{:});
     end
     function varargout = computeChildVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1036, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1040, self, varargin{:});
     end
     function varargout = computeChildVelAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1037, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1041, self, varargin{:});
     end
     function varargout = computeChildAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1038, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1042, self, varargin{:});
     end
     function varargout = computeChildBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1039, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1043, self, varargin{:});
     end
     function varargout = computeJointTorque(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1040, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1044, self, varargin{:});
     end
     function varargout = hasPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1041, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1045, self, varargin{:});
     end
     function varargout = enablePosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1042, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1046, self, varargin{:});
     end
     function varargout = getPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1043, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1047, self, varargin{:});
     end
     function varargout = getMinPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1044, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1048, self, varargin{:});
     end
     function varargout = getMaxPosLimit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1045, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1049, self, varargin{:});
     end
     function varargout = setPosLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1046, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1050, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/Rotation.m b/bindings/matlab/autogenerated/+iDynTree/Rotation.m
index cdcf4e745fe..e5203b4bfd7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Rotation.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Rotation.m
@@ -52,14 +52,14 @@
       [varargout{1:nargout}] = iDynTreeMEX(760, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(773, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(777, self, varargin{:});
     end
     function varargout = display(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(774, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(778, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(775, self);
+        iDynTreeMEX(779, self);
         self.SwigClear();
       end
     end
@@ -92,20 +92,32 @@ function delete(self)
     function varargout = RPYRightTrivializedDerivative(varargin)
      [varargout{1:nargout}] = iDynTreeMEX(767, varargin{:});
     end
-    function varargout = RPYRightTrivializedDerivativeInverse(varargin)
+    function varargout = RPYRightTrivializedDerivativeRateOfChange(varargin)
      [varargout{1:nargout}] = iDynTreeMEX(768, varargin{:});
     end
-    function varargout = QuaternionRightTrivializedDerivative(varargin)
+    function varargout = RPYRightTrivializedDerivativeInverse(varargin)
      [varargout{1:nargout}] = iDynTreeMEX(769, varargin{:});
     end
-    function varargout = QuaternionRightTrivializedDerivativeInverse(varargin)
+    function varargout = RPYRightTrivializedDerivativeInverseRateOfChange(varargin)
      [varargout{1:nargout}] = iDynTreeMEX(770, varargin{:});
     end
-    function varargout = Identity(varargin)
+    function varargout = QuaternionRightTrivializedDerivative(varargin)
      [varargout{1:nargout}] = iDynTreeMEX(771, varargin{:});
     end
-    function varargout = RotationFromQuaternion(varargin)
+    function varargout = QuaternionRightTrivializedDerivativeInverse(varargin)
      [varargout{1:nargout}] = iDynTreeMEX(772, varargin{:});
     end
+    function varargout = Identity(varargin)
+     [varargout{1:nargout}] = iDynTreeMEX(773, varargin{:});
+    end
+    function varargout = RotationFromQuaternion(varargin)
+     [varargout{1:nargout}] = iDynTreeMEX(774, varargin{:});
+    end
+    function varargout = leftJacobian(varargin)
+     [varargout{1:nargout}] = iDynTreeMEX(775, varargin{:});
+    end
+    function varargout = leftJacobianInverse(varargin)
+     [varargout{1:nargout}] = iDynTreeMEX(776, varargin{:});
+    end
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Sensor.m b/bindings/matlab/autogenerated/+iDynTree/Sensor.m
index 4a500232632..0550c8521e1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Sensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Sensor.m
@@ -5,33 +5,33 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1290, self);
+        iDynTreeMEX(1298, self);
         self.SwigClear();
       end
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1291, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1299, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1292, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1300, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1293, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1301, self, varargin{:});
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1294, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1302, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1295, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1303, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1296, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1304, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1297, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1305, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1298, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1306, self, varargin{:});
     end
     function self = Sensor(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/SensorsList.m b/bindings/matlab/autogenerated/+iDynTree/SensorsList.m
index addfdc70612..8c4dbdd05e8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SensorsList.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SensorsList.m
@@ -9,61 +9,61 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1313, varargin{:});
+        tmp = iDynTreeMEX(1321, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1314, self);
+        iDynTreeMEX(1322, self);
         self.SwigClear();
       end
     end
     function varargout = addSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1315, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1323, self, varargin{:});
     end
     function varargout = setSerialization(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1316, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1324, self, varargin{:});
     end
     function varargout = getSerialization(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1317, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1325, self, varargin{:});
     end
     function varargout = getNrOfSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1318, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1326, self, varargin{:});
     end
     function varargout = getSensorIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1319, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1327, self, varargin{:});
     end
     function varargout = getSizeOfAllSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1320, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1328, self, varargin{:});
     end
     function varargout = getSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1321, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1329, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1322, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1330, self, varargin{:});
     end
     function varargout = removeSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1323, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1331, self, varargin{:});
     end
     function varargout = removeAllSensorsOfType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1324, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1332, self, varargin{:});
     end
     function varargout = getSixAxisForceTorqueSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1325, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1333, self, varargin{:});
     end
     function varargout = getAccelerometerSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1326, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1334, self, varargin{:});
     end
     function varargout = getGyroscopeSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1327, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1335, self, varargin{:});
     end
     function varargout = getThreeAxisAngularAccelerometerSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1328, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1336, self, varargin{:});
     end
     function varargout = getThreeAxisForceTorqueContactSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1329, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1337, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m b/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m
index 609dfc1233a..01bd36d6152 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SensorsMeasurements.m
@@ -9,37 +9,37 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1330, varargin{:});
+        tmp = iDynTreeMEX(1338, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1331, self);
+        iDynTreeMEX(1339, self);
         self.SwigClear();
       end
     end
     function varargout = setNrOfSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1332, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1340, self, varargin{:});
     end
     function varargout = getNrOfSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1333, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1341, self, varargin{:});
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1334, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1342, self, varargin{:});
     end
     function varargout = toVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1335, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1343, self, varargin{:});
     end
     function varargout = setMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1336, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1344, self, varargin{:});
     end
     function varargout = getMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1337, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1345, self, varargin{:});
     end
     function varargout = getSizeOfAllSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1338, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1346, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m b/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
index bb8d1653b26..fcb310a459b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
@@ -9,43 +9,46 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1527, varargin{:});
+        tmp = iDynTreeMEX(1551, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1528, self);
+        iDynTreeMEX(1552, self);
         self.SwigClear();
       end
     end
     function varargout = setModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1529, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1553, self, varargin{:});
     end
     function varargout = loadModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1530, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1554, self, varargin{:});
     end
     function varargout = loadModelFromFileWithSpecifiedDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1531, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1555, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1532, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1556, self, varargin{:});
     end
     function varargout = updateKinematics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1533, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1557, self, varargin{:});
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1534, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1558, self, varargin{:});
     end
     function varargout = changeFixedFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1535, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1559, self, varargin{:});
     end
     function varargout = getCurrentFixedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1536, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1560, self, varargin{:});
     end
     function varargout = getWorldLinkTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1537, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1561, self, varargin{:});
+    end
+    function varargout = getWorldFrameTransform(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1562, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m b/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m
index 3bea68b5f6d..7c12047d0c6 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SixAxisForceTorqueSensor.m
@@ -7,100 +7,100 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1339, varargin{:});
+        tmp = iDynTreeMEX(1347, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1340, self);
+        iDynTreeMEX(1348, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1341, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1349, self, varargin{:});
     end
     function varargout = setFirstLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1342, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1350, self, varargin{:});
     end
     function varargout = setSecondLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1343, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1351, self, varargin{:});
     end
     function varargout = getFirstLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1344, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1352, self, varargin{:});
     end
     function varargout = getSecondLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1345, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1353, self, varargin{:});
     end
     function varargout = setFirstLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1346, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1354, self, varargin{:});
     end
     function varargout = setSecondLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1347, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1355, self, varargin{:});
     end
     function varargout = getFirstLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1348, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1356, self, varargin{:});
     end
     function varargout = getSecondLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1349, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1357, self, varargin{:});
     end
     function varargout = setParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1350, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1358, self, varargin{:});
     end
     function varargout = setParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1351, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1359, self, varargin{:});
     end
     function varargout = setAppliedWrenchLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1352, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1360, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1353, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1361, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1354, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1362, self, varargin{:});
     end
     function varargout = getParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1355, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1363, self, varargin{:});
     end
     function varargout = getParentJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1356, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1364, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1357, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1365, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1358, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1366, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1359, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1367, self, varargin{:});
     end
     function varargout = updateIndeces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1360, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1368, self, varargin{:});
     end
     function varargout = getAppliedWrenchLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1361, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1369, self, varargin{:});
     end
     function varargout = isLinkAttachedToSensor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1362, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1370, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1363, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1371, self, varargin{:});
     end
     function varargout = getWrenchAppliedOnLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1364, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1372, self, varargin{:});
     end
     function varargout = getWrenchAppliedOnLinkMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1365, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1373, self, varargin{:});
     end
     function varargout = getWrenchAppliedOnLinkInverseMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1366, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1374, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1367, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1375, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1368, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1376, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/SolidShape.m b/bindings/matlab/autogenerated/+iDynTree/SolidShape.m
index 6cbc10a7038..75f5231a61b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SolidShape.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SolidShape.m
@@ -5,66 +5,76 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1089, self);
+        iDynTreeMEX(1093, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1090, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1094, self, varargin{:});
     end
     function varargout = name(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1091, self);
+        varargout{1} = iDynTreeMEX(1095, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1092, self, varargin{1});
+        iDynTreeMEX(1096, self, varargin{1});
+      end
+    end
+    function varargout = nameIsValid(self, varargin)
+      narginchk(1, 2)
+      if nargin==1
+        nargoutchk(0, 1)
+        varargout{1} = iDynTreeMEX(1097, self);
+      else
+        nargoutchk(0, 0)
+        iDynTreeMEX(1098, self, varargin{1});
       end
     end
     function varargout = link_H_geometry(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1093, self);
+        varargout{1} = iDynTreeMEX(1099, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1094, self, varargin{1});
+        iDynTreeMEX(1100, self, varargin{1});
       end
     end
     function varargout = material(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1095, self);
+        varargout{1} = iDynTreeMEX(1101, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1096, self, varargin{1});
+        iDynTreeMEX(1102, self, varargin{1});
       end
     end
     function varargout = isSphere(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1097, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1103, self, varargin{:});
     end
     function varargout = isBox(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1098, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1104, self, varargin{:});
     end
     function varargout = isCylinder(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1099, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1105, self, varargin{:});
     end
     function varargout = isExternalMesh(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1100, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1106, self, varargin{:});
     end
     function varargout = asSphere(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1101, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1107, self, varargin{:});
     end
     function varargout = asBox(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1102, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1108, self, varargin{:});
     end
     function varargout = asCylinder(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1103, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1109, self, varargin{:});
     end
     function varargout = asExternalMesh(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1104, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1110, self, varargin{:});
     end
     function self = SolidShape(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/Sphere.m b/bindings/matlab/autogenerated/+iDynTree/Sphere.m
index 15620360639..fdb18b44f1d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Sphere.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Sphere.m
@@ -2,21 +2,21 @@
   methods
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1105, self);
+        iDynTreeMEX(1111, self);
         self.SwigClear();
       end
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1106, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1112, self, varargin{:});
     end
     function varargout = radius(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1107, self);
+        varargout{1} = iDynTreeMEX(1113, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1108, self, varargin{1});
+        iDynTreeMEX(1114, self, varargin{1});
       end
     end
     function self = Sphere(varargin)
@@ -26,7 +26,7 @@ function delete(self)
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1109, varargin{:});
+        tmp = iDynTreeMEX(1115, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
diff --git a/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m b/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m
index 643baa28a1a..84473427c2b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m
+++ b/bindings/matlab/autogenerated/+iDynTree/TRAVERSAL_INVALID_INDEX.m
@@ -2,9 +2,9 @@
   narginchk(0,1)
   if nargin==0
     nargoutchk(0,1)
-    varargout{1} = iDynTreeMEX(855);
+    varargout{1} = iDynTreeMEX(859);
   else
     nargoutchk(0,0)
-    iDynTreeMEX(856,varargin{1});
+    iDynTreeMEX(860,varargin{1});
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m
index 957361e30d2..f4c7585fa70 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisAngularAccelerometerSensor.m
@@ -7,55 +7,55 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1401, varargin{:});
+        tmp = iDynTreeMEX(1409, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1402, self);
+        iDynTreeMEX(1410, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1403, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1411, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1404, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1412, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1405, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1413, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1406, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1414, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1407, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1415, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1408, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1416, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1409, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1417, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1410, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1418, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1411, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1419, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1412, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1420, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1413, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1421, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1414, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1422, self, varargin{:});
     end
     function varargout = predictMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1415, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1423, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m
index 2fa14024d4f..3ec094f03b8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ThreeAxisForceTorqueContactSensor.m
@@ -7,64 +7,64 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1416, varargin{:});
+        tmp = iDynTreeMEX(1424, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1417, self);
+        iDynTreeMEX(1425, self);
         self.SwigClear();
       end
     end
     function varargout = setName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1418, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1426, self, varargin{:});
     end
     function varargout = setLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1419, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1427, self, varargin{:});
     end
     function varargout = setParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1420, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1428, self, varargin{:});
     end
     function varargout = setParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1421, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1429, self, varargin{:});
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1422, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1430, self, varargin{:});
     end
     function varargout = getSensorType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1423, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1431, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1424, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1432, self, varargin{:});
     end
     function varargout = getParentLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1425, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1433, self, varargin{:});
     end
     function varargout = getLinkSensorTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1426, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1434, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1427, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1435, self, varargin{:});
     end
     function varargout = clone(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1428, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1436, self, varargin{:});
     end
     function varargout = updateIndices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1429, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1437, self, varargin{:});
     end
     function varargout = setLoadCellLocations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1430, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1438, self, varargin{:});
     end
     function varargout = getLoadCellLocations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1431, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1439, self, varargin{:});
     end
     function varargout = computeThreeAxisForceTorqueFromLoadCellMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1432, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1440, self, varargin{:});
     end
     function varargout = computeCenterOfPressureFromLoadCellMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1433, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1441, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/Transform.m b/bindings/matlab/autogenerated/+iDynTree/Transform.m
index 3da637783e5..2fc9834b5c1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Transform.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Transform.m
@@ -9,69 +9,69 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(784, varargin{:});
+        tmp = iDynTreeMEX(788, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = fromHomogeneousTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(785, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(789, self, varargin{:});
     end
     function varargout = getSemantics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(786, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(790, self, varargin{:});
     end
     function varargout = getRotation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(787, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(791, self, varargin{:});
     end
     function varargout = getPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(788, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(792, self, varargin{:});
     end
     function varargout = setRotation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(789, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(793, self, varargin{:});
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(790, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(794, self, varargin{:});
     end
     function varargout = inverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(793, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(797, self, varargin{:});
     end
     function varargout = mtimes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(794, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(798, self, varargin{:});
     end
     function varargout = asHomogeneousTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(796, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(800, self, varargin{:});
     end
     function varargout = asAdjointTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(797, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(801, self, varargin{:});
     end
     function varargout = asAdjointTransformWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(798, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(802, self, varargin{:});
     end
     function varargout = log(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(799, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(803, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(800, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(804, self, varargin{:});
     end
     function varargout = display(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(801, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(805, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(802, self);
+        iDynTreeMEX(806, self);
         self.SwigClear();
       end
     end
   end
   methods(Static)
     function varargout = compose(varargin)
-     [varargout{1:nargout}] = iDynTreeMEX(791, varargin{:});
+     [varargout{1:nargout}] = iDynTreeMEX(795, varargin{:});
     end
     function varargout = inverse2(varargin)
-     [varargout{1:nargout}] = iDynTreeMEX(792, varargin{:});
+     [varargout{1:nargout}] = iDynTreeMEX(796, varargin{:});
     end
     function varargout = Identity(varargin)
-     [varargout{1:nargout}] = iDynTreeMEX(795, varargin{:});
+     [varargout{1:nargout}] = iDynTreeMEX(799, varargin{:});
     end
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/TransformDerivative.m b/bindings/matlab/autogenerated/+iDynTree/TransformDerivative.m
index 7f8d1deb6ec..4da81ba5259 100644
--- a/bindings/matlab/autogenerated/+iDynTree/TransformDerivative.m
+++ b/bindings/matlab/autogenerated/+iDynTree/TransformDerivative.m
@@ -9,51 +9,51 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(803, varargin{:});
+        tmp = iDynTreeMEX(807, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(804, self);
+        iDynTreeMEX(808, self);
         self.SwigClear();
       end
     end
     function varargout = getRotationDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(805, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(809, self, varargin{:});
     end
     function varargout = getPositionDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(806, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(810, self, varargin{:});
     end
     function varargout = setRotationDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(807, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(811, self, varargin{:});
     end
     function varargout = setPositionDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(808, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(812, self, varargin{:});
     end
     function varargout = asHomogeneousTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(810, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(814, self, varargin{:});
     end
     function varargout = asAdjointTransformDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(811, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(815, self, varargin{:});
     end
     function varargout = asAdjointTransformWrenchDerivative(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(812, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(816, self, varargin{:});
     end
     function varargout = mtimes(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(813, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(817, self, varargin{:});
     end
     function varargout = derivativeOfInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(814, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(818, self, varargin{:});
     end
     function varargout = transform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(815, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(819, self, varargin{:});
     end
   end
   methods(Static)
     function varargout = Zero(varargin)
-     [varargout{1:nargout}] = iDynTreeMEX(809, varargin{:});
+     [varargout{1:nargout}] = iDynTreeMEX(813, varargin{:});
     end
   end
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/TransformSemantics.m b/bindings/matlab/autogenerated/+iDynTree/TransformSemantics.m
index 8b11819f16d..1787f498e9b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/TransformSemantics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/TransformSemantics.m
@@ -9,32 +9,32 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(776, varargin{:});
+        tmp = iDynTreeMEX(780, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = getRotationSemantics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(777, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(781, self, varargin{:});
     end
     function varargout = getPositionSemantics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(778, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(782, self, varargin{:});
     end
     function varargout = setRotationSemantics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(779, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(783, self, varargin{:});
     end
     function varargout = setPositionSemantics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(780, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(784, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(781, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(785, self, varargin{:});
     end
     function varargout = display(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(782, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(786, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(783, self);
+        iDynTreeMEX(787, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Traversal.m b/bindings/matlab/autogenerated/+iDynTree/Traversal.m
index da1e002d234..fac6d8dfc44 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Traversal.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Traversal.m
@@ -9,61 +9,61 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1072, varargin{:});
+        tmp = iDynTreeMEX(1076, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1073, self);
+        iDynTreeMEX(1077, self);
         self.SwigClear();
       end
     end
     function varargout = getNrOfVisitedLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1074, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1078, self, varargin{:});
     end
     function varargout = getLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1075, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1079, self, varargin{:});
     end
     function varargout = getBaseLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1076, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1080, self, varargin{:});
     end
     function varargout = getParentLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1077, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1081, self, varargin{:});
     end
     function varargout = getParentJoint(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1078, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1082, self, varargin{:});
     end
     function varargout = getParentLinkFromLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1079, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1083, self, varargin{:});
     end
     function varargout = getParentJointFromLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1080, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1084, self, varargin{:});
     end
     function varargout = getTraversalIndexFromLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1081, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1085, self, varargin{:});
     end
     function varargout = reset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1082, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1086, self, varargin{:});
     end
     function varargout = addTraversalBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1083, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1087, self, varargin{:});
     end
     function varargout = addTraversalElement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1084, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1088, self, varargin{:});
     end
     function varargout = isParentOf(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1085, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1089, self, varargin{:});
     end
     function varargout = getChildLinkIndexFromJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1086, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1090, self, varargin{:});
     end
     function varargout = getParentLinkIndexFromJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1087, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1091, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1088, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1092, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m b/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m
index bdb2428fd6d..6f8a4e3e588 100644
--- a/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/URDFParserOptions.m
@@ -7,20 +7,20 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1436, self);
+        varargout{1} = iDynTreeMEX(1444, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1437, self, varargin{1});
+        iDynTreeMEX(1445, self, varargin{1});
       end
     end
     function varargout = originalFilename(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1438, self);
+        varargout{1} = iDynTreeMEX(1446, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1439, self, varargin{1});
+        iDynTreeMEX(1447, self, varargin{1});
       end
     end
     function self = URDFParserOptions(varargin)
@@ -29,14 +29,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1440, varargin{:});
+        tmp = iDynTreeMEX(1448, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1441, self);
+        iDynTreeMEX(1449, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m b/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
index 2a98a8da0f0..89eaba4eec3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
+++ b/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
@@ -9,7 +9,7 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1466, varargin{:});
+        tmp = iDynTreeMEX(1490, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
@@ -18,55 +18,55 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1467, self);
+        varargout{1} = iDynTreeMEX(1491, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1468, self, varargin{1});
+        iDynTreeMEX(1492, self, varargin{1});
       end
     end
     function varargout = contactPoint(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1469, self);
+        varargout{1} = iDynTreeMEX(1493, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1470, self, varargin{1});
+        iDynTreeMEX(1494, self, varargin{1});
       end
     end
     function varargout = forceDirection(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1471, self);
+        varargout{1} = iDynTreeMEX(1495, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1472, self, varargin{1});
+        iDynTreeMEX(1496, self, varargin{1});
       end
     end
     function varargout = knownWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1473, self);
+        varargout{1} = iDynTreeMEX(1497, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1474, self, varargin{1});
+        iDynTreeMEX(1498, self, varargin{1});
       end
     end
     function varargout = contactId(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1475, self);
+        varargout{1} = iDynTreeMEX(1499, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1476, self, varargin{1});
+        iDynTreeMEX(1500, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1477, self);
+        iDynTreeMEX(1501, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Visualizer.m b/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
index 94117fe3ad6..12ace447c7e 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
@@ -9,55 +9,55 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1930, varargin{:});
+        tmp = iDynTreeMEX(1956, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1931, self);
+        iDynTreeMEX(1957, self);
         self.SwigClear();
       end
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1932, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1958, self, varargin{:});
     end
     function varargout = getNrOfVisualizedModels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1933, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1959, self, varargin{:});
     end
     function varargout = getModelInstanceName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1934, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1960, self, varargin{:});
     end
     function varargout = getModelInstanceIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1935, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1961, self, varargin{:});
     end
     function varargout = addModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1936, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1962, self, varargin{:});
     end
     function varargout = modelViz(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1937, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1963, self, varargin{:});
     end
     function varargout = camera(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1938, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1964, self, varargin{:});
     end
     function varargout = enviroment(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1939, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1965, self, varargin{:});
     end
     function varargout = vectors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1940, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1966, self, varargin{:});
     end
     function varargout = run(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1941, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1967, self, varargin{:});
     end
     function varargout = draw(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1942, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1968, self, varargin{:});
     end
     function varargout = drawToFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1943, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1969, self, varargin{:});
     end
     function varargout = close(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1944, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1970, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m b/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
index da9ac10c600..e756db5a577 100644
--- a/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1920, self);
+        varargout{1} = iDynTreeMEX(1946, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1921, self, varargin{1});
+        iDynTreeMEX(1947, self, varargin{1});
       end
     end
     function varargout = winWidth(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1922, self);
+        varargout{1} = iDynTreeMEX(1948, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1923, self, varargin{1});
+        iDynTreeMEX(1949, self, varargin{1});
       end
     end
     function varargout = winHeight(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1924, self);
+        varargout{1} = iDynTreeMEX(1950, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1925, self, varargin{1});
+        iDynTreeMEX(1951, self, varargin{1});
       end
     end
     function varargout = rootFrameArrowsDimension(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1926, self);
+        varargout{1} = iDynTreeMEX(1952, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1927, self, varargin{1});
+        iDynTreeMEX(1953, self, varargin{1});
       end
     end
     function self = VisualizerOptions(varargin)
@@ -49,14 +49,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1928, varargin{:});
+        tmp = iDynTreeMEX(1954, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1929, self);
+        iDynTreeMEX(1955, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m b/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
index b63c5945733..71dff558717 100644
--- a/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
+++ b/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
@@ -1,3 +1,3 @@
 function varargout = computeLinkNetWrenchesWithoutGravity(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1511, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1535, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m
index f96b7ef17c9..7846688c5d3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDF.m
@@ -1,3 +1,3 @@
 function varargout = dofsListFromURDF(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1444, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1452, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m
index 99c895327ab..e6951de4635 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dofsListFromURDFString.m
@@ -1,3 +1,3 @@
 function varargout = dofsListFromURDFString(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1445, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1453, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m b/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m
index 34ff538ed5d..8b2a15e731e 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamic_extent.m
@@ -1,3 +1,3 @@
 function v = dynamic_extent()
-  v = iDynTreeMEX(816);
+  v = iDynTreeMEX(820);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
index d18bed729d4..da259fdf660 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = dynamicsEstimationForwardVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1509, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1533, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
index dde939d756e..c301eba8cce 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
@@ -1,3 +1,3 @@
 function varargout = dynamicsEstimationForwardVelKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1510, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1534, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
index 96f63289218..0849c07d164 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
@@ -1,3 +1,3 @@
 function varargout = estimateExternalWrenches(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1508, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1532, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
index 90fc76b0daa..e29c2a0fcd6 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
@@ -9,86 +9,86 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1489, varargin{:});
+        tmp = iDynTreeMEX(1513, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1490, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1514, self, varargin{:});
     end
     function varargout = getNrOfSubModels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1491, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1515, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1492, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1516, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1493, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1517, self, varargin{:});
     end
     function varargout = A(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1494, self);
+        varargout{1} = iDynTreeMEX(1518, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1495, self, varargin{1});
+        iDynTreeMEX(1519, self, varargin{1});
       end
     end
     function varargout = x(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1496, self);
+        varargout{1} = iDynTreeMEX(1520, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1497, self, varargin{1});
+        iDynTreeMEX(1521, self, varargin{1});
       end
     end
     function varargout = b(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1498, self);
+        varargout{1} = iDynTreeMEX(1522, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1499, self, varargin{1});
+        iDynTreeMEX(1523, self, varargin{1});
       end
     end
     function varargout = pinvA(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1500, self);
+        varargout{1} = iDynTreeMEX(1524, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1501, self, varargin{1});
+        iDynTreeMEX(1525, self, varargin{1});
       end
     end
     function varargout = b_contacts_subtree(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1502, self);
+        varargout{1} = iDynTreeMEX(1526, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1503, self, varargin{1});
+        iDynTreeMEX(1527, self, varargin{1});
       end
     end
     function varargout = subModelBase_H_link(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1504, self);
+        varargout{1} = iDynTreeMEX(1528, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1505, self, varargin{1});
+        iDynTreeMEX(1529, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1506, self);
+        iDynTreeMEX(1530, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
index 092ff059b6d..15f214467cf 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
@@ -1,3 +1,3 @@
 function varargout = estimateExternalWrenchesWithoutInternalFT(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1507, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1531, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m b/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m
new file mode 100644
index 00000000000..f40cfe8d0c3
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m
@@ -0,0 +1,3 @@
+function varargout = estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(varargin)
+  [varargout{1:nargout}] = iDynTreeMEX(1771, varargin{:});
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m b/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
index 35d21940e16..40b419ddbe1 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
@@ -1,3 +1,3 @@
 function varargout = estimateLinkContactWrenchesFromLinkNetExternalWrenches(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1512, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1536, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m b/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m
index 581f79578bf..c791ac1fb7b 100644
--- a/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m
+++ b/bindings/matlab/autogenerated/+iDynTree/getSensorTypeSize.m
@@ -1,3 +1,3 @@
 function varargout = getSensorTypeSize(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1289, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1297, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m b/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
index 7254e435cc5..16d7ce70d84 100644
--- a/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
@@ -1,3 +1,3 @@
 function v = input_dimensions()
-  v = iDynTreeMEX(1693);
+  v = iDynTreeMEX(1726);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
index a27b2a29046..6eb5622f355 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isDOFBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1540, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1565, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
index e9f0fc52c67..68b28970e6c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isJointBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1539, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1564, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m b/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m
index 0cd34c1f019..ce92735879a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isJointSensor.m
@@ -1,3 +1,3 @@
 function varargout = isJointSensor(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1288, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1296, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
index 5c74c2dc43b..ca1ec7e7207 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isLinkBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1538, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1563, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m b/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m
index b3a289061f8..acc9a6523d5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isLinkSensor.m
@@ -1,3 +1,3 @@
 function varargout = isLinkSensor(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1287, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1295, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m b/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m
index 7057f74b286..6bbfee7412f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/modelFromURDF.m
@@ -1,3 +1,3 @@
 function varargout = modelFromURDF(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1442, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1450, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m b/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m
index 143d62b7213..f07fd655851 100644
--- a/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m
+++ b/bindings/matlab/autogenerated/+iDynTree/modelFromURDFString.m
@@ -1,3 +1,3 @@
 function varargout = modelFromURDFString(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1443, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1451, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
index c599cf3b597..0b0c0b44d2f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
@@ -1,3 +1,3 @@
 function v = output_dimensions_with_magnetometer()
-  v = iDynTreeMEX(1691);
+  v = iDynTreeMEX(1724);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
index 10a1e1f01a0..375a71cd363 100644
--- a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
@@ -1,3 +1,3 @@
 function v = output_dimensions_without_magnetometer()
-  v = iDynTreeMEX(1692);
+  v = iDynTreeMEX(1725);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m
index 49b9e415caf..0b78483735d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m
+++ b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurements.m
@@ -1,3 +1,3 @@
 function varargout = predictSensorsMeasurements(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1434, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1442, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m
index 4f9a173b471..aa930a6e4c9 100644
--- a/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/predictSensorsMeasurementsFromRawBuffers.m
@@ -1,3 +1,3 @@
 function varargout = predictSensorsMeasurementsFromRawBuffers(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1435, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1443, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m
index 8608cad5326..5e6af42593a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDF.m
@@ -1,3 +1,3 @@
 function varargout = sensorsFromURDF(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1446, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1454, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m
index efb50b66751..85a47234fc5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m
+++ b/bindings/matlab/autogenerated/+iDynTree/sensorsFromURDFString.m
@@ -1,3 +1,3 @@
 function varargout = sensorsFromURDFString(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1447, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1455, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
index 8850ccd97a5..52394cc4d0f 100644
--- a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
+++ b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
@@ -1325,104 +1325,108 @@ namespace swig {
 #define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_1_t swig_types[113]
 #define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_6_t swig_types[114]
 #define SWIGTYPE_p_iDynTree__Model swig_types[115]
-#define SWIGTYPE_p_iDynTree__ModelLoader swig_types[116]
-#define SWIGTYPE_p_iDynTree__ModelParserOptions swig_types[117]
-#define SWIGTYPE_p_iDynTree__ModelSolidShapes swig_types[118]
-#define SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian swig_types[119]
-#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t swig_types[120]
-#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t swig_types[121]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t swig_types[122]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t swig_types[123]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t swig_types[124]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t swig_types[125]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t swig_types[126]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t swig_types[127]
-#define SWIGTYPE_p_iDynTree__Neighbor swig_types[128]
-#define SWIGTYPE_p_iDynTree__Position swig_types[129]
-#define SWIGTYPE_p_iDynTree__PositionRaw swig_types[130]
-#define SWIGTYPE_p_iDynTree__PositionSemantics swig_types[131]
-#define SWIGTYPE_p_iDynTree__PrismaticJoint swig_types[132]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorGenerator swig_types[133]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParameter swig_types[134]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParametersList swig_types[135]
-#define SWIGTYPE_p_iDynTree__RevoluteJoint swig_types[136]
-#define SWIGTYPE_p_iDynTree__RigidBodyInertiaNonLinearParametrization swig_types[137]
-#define SWIGTYPE_p_iDynTree__Rotation swig_types[138]
-#define SWIGTYPE_p_iDynTree__RotationRaw swig_types[139]
-#define SWIGTYPE_p_iDynTree__RotationSemantics swig_types[140]
-#define SWIGTYPE_p_iDynTree__RotationalInertiaRaw swig_types[141]
-#define SWIGTYPE_p_iDynTree__Sensor swig_types[142]
-#define SWIGTYPE_p_iDynTree__SensorsList swig_types[143]
-#define SWIGTYPE_p_iDynTree__SensorsMeasurements swig_types[144]
-#define SWIGTYPE_p_iDynTree__SimpleLeggedOdometry swig_types[145]
-#define SWIGTYPE_p_iDynTree__SixAxisForceTorqueSensor swig_types[146]
-#define SWIGTYPE_p_iDynTree__SolidShape swig_types[147]
-#define SWIGTYPE_p_iDynTree__SpanT_double__1_t swig_types[148]
-#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t swig_types[149]
-#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t swig_types[150]
-#define SWIGTYPE_p_iDynTree__SpatialAcc swig_types[151]
-#define SWIGTYPE_p_iDynTree__SpatialForceVector swig_types[152]
-#define SWIGTYPE_p_iDynTree__SpatialInertia swig_types[153]
-#define SWIGTYPE_p_iDynTree__SpatialInertiaRaw swig_types[154]
-#define SWIGTYPE_p_iDynTree__SpatialMomentum swig_types[155]
-#define SWIGTYPE_p_iDynTree__SpatialMotionVector swig_types[156]
-#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearForceVector3Semantics_iDynTree__AngularForceVector3Semantics_t swig_types[157]
-#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearMotionVector3Semantics_iDynTree__AngularMotionVector3Semantics_t swig_types[158]
-#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t swig_types[159]
-#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t swig_types[160]
-#define SWIGTYPE_p_iDynTree__Sphere swig_types[161]
-#define SWIGTYPE_p_iDynTree__SubModelDecomposition swig_types[162]
-#define SWIGTYPE_p_iDynTree__ThreeAxisAngularAccelerometerSensor swig_types[163]
-#define SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor swig_types[164]
-#define SWIGTYPE_p_iDynTree__Transform swig_types[165]
-#define SWIGTYPE_p_iDynTree__TransformDerivative swig_types[166]
-#define SWIGTYPE_p_iDynTree__TransformSemantics swig_types[167]
-#define SWIGTYPE_p_iDynTree__Traversal swig_types[168]
-#define SWIGTYPE_p_iDynTree__Triplets swig_types[169]
-#define SWIGTYPE_p_iDynTree__Twist swig_types[170]
-#define SWIGTYPE_p_iDynTree__URDFParserOptions swig_types[171]
-#define SWIGTYPE_p_iDynTree__UnknownWrenchContact swig_types[172]
-#define SWIGTYPE_p_iDynTree__VectorDynSize swig_types[173]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_10_t swig_types[174]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_16_t swig_types[175]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_2_t swig_types[176]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t swig_types[177]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t swig_types[178]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t swig_types[179]
-#define SWIGTYPE_p_iDynTree__Visualizer swig_types[180]
-#define SWIGTYPE_p_iDynTree__VisualizerOptions swig_types[181]
-#define SWIGTYPE_p_iDynTree__Wrench swig_types[182]
-#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t swig_types[183]
-#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t swig_types[184]
-#define SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers swig_types[185]
-#define SWIGTYPE_p_index_type swig_types[186]
-#define SWIGTYPE_p_int swig_types[187]
-#define SWIGTYPE_p_iterator swig_types[188]
-#define SWIGTYPE_p_pointer swig_types[189]
-#define SWIGTYPE_p_reference swig_types[190]
-#define SWIGTYPE_p_reverse_iterator swig_types[191]
-#define SWIGTYPE_p_size_type swig_types[192]
-#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t swig_types[193]
-#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdySensor_t swig_types[194]
-#define SWIGTYPE_p_std__allocatorT_std__string_t swig_types[195]
-#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t swig_types[196]
-#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t swig_types[197]
-#define SWIGTYPE_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t swig_types[198]
-#define SWIGTYPE_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t swig_types[199]
-#define SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t swig_types[200]
-#define SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t swig_types[201]
-#define SWIGTYPE_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t swig_types[202]
-#define SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t swig_types[203]
-#define SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t swig_types[204]
-#define SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t swig_types[205]
-#define SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t swig_types[206]
-#define SWIGTYPE_p_swig__MatlabSwigIterator swig_types[207]
-#define SWIGTYPE_p_typed_iterator swig_types[208]
-#define SWIGTYPE_p_unsigned_int swig_types[209]
-#define SWIGTYPE_p_unsigned_long swig_types[210]
-#define SWIGTYPE_p_value_type swig_types[211]
-static swig_type_info *swig_types[213];
-static swig_module_info swig_module = {swig_types, 212, 0, 0, 0, 0};
+#define SWIGTYPE_p_iDynTree__ModelExporter swig_types[116]
+#define SWIGTYPE_p_iDynTree__ModelExporterOptions swig_types[117]
+#define SWIGTYPE_p_iDynTree__ModelLoader swig_types[118]
+#define SWIGTYPE_p_iDynTree__ModelParserOptions swig_types[119]
+#define SWIGTYPE_p_iDynTree__ModelSolidShapes swig_types[120]
+#define SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian swig_types[121]
+#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t swig_types[122]
+#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t swig_types[123]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t swig_types[124]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t swig_types[125]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t swig_types[126]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t swig_types[127]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t swig_types[128]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t swig_types[129]
+#define SWIGTYPE_p_iDynTree__Neighbor swig_types[130]
+#define SWIGTYPE_p_iDynTree__Position swig_types[131]
+#define SWIGTYPE_p_iDynTree__PositionRaw swig_types[132]
+#define SWIGTYPE_p_iDynTree__PositionSemantics swig_types[133]
+#define SWIGTYPE_p_iDynTree__PrismaticJoint swig_types[134]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorGenerator swig_types[135]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParameter swig_types[136]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParametersList swig_types[137]
+#define SWIGTYPE_p_iDynTree__RevoluteJoint swig_types[138]
+#define SWIGTYPE_p_iDynTree__RigidBodyInertiaNonLinearParametrization swig_types[139]
+#define SWIGTYPE_p_iDynTree__Rotation swig_types[140]
+#define SWIGTYPE_p_iDynTree__RotationRaw swig_types[141]
+#define SWIGTYPE_p_iDynTree__RotationSemantics swig_types[142]
+#define SWIGTYPE_p_iDynTree__RotationalInertiaRaw swig_types[143]
+#define SWIGTYPE_p_iDynTree__Sensor swig_types[144]
+#define SWIGTYPE_p_iDynTree__SensorsList swig_types[145]
+#define SWIGTYPE_p_iDynTree__SensorsMeasurements swig_types[146]
+#define SWIGTYPE_p_iDynTree__SimpleLeggedOdometry swig_types[147]
+#define SWIGTYPE_p_iDynTree__SixAxisForceTorqueSensor swig_types[148]
+#define SWIGTYPE_p_iDynTree__SolidShape swig_types[149]
+#define SWIGTYPE_p_iDynTree__SpanT_double__1_t swig_types[150]
+#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t swig_types[151]
+#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t swig_types[152]
+#define SWIGTYPE_p_iDynTree__SpatialAcc swig_types[153]
+#define SWIGTYPE_p_iDynTree__SpatialForceVector swig_types[154]
+#define SWIGTYPE_p_iDynTree__SpatialInertia swig_types[155]
+#define SWIGTYPE_p_iDynTree__SpatialInertiaRaw swig_types[156]
+#define SWIGTYPE_p_iDynTree__SpatialMomentum swig_types[157]
+#define SWIGTYPE_p_iDynTree__SpatialMotionVector swig_types[158]
+#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearForceVector3Semantics_iDynTree__AngularForceVector3Semantics_t swig_types[159]
+#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearMotionVector3Semantics_iDynTree__AngularMotionVector3Semantics_t swig_types[160]
+#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t swig_types[161]
+#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t swig_types[162]
+#define SWIGTYPE_p_iDynTree__Sphere swig_types[163]
+#define SWIGTYPE_p_iDynTree__SubModelDecomposition swig_types[164]
+#define SWIGTYPE_p_iDynTree__ThreeAxisAngularAccelerometerSensor swig_types[165]
+#define SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor swig_types[166]
+#define SWIGTYPE_p_iDynTree__Transform swig_types[167]
+#define SWIGTYPE_p_iDynTree__TransformDerivative swig_types[168]
+#define SWIGTYPE_p_iDynTree__TransformSemantics swig_types[169]
+#define SWIGTYPE_p_iDynTree__Traversal swig_types[170]
+#define SWIGTYPE_p_iDynTree__Triplets swig_types[171]
+#define SWIGTYPE_p_iDynTree__Twist swig_types[172]
+#define SWIGTYPE_p_iDynTree__URDFParserOptions swig_types[173]
+#define SWIGTYPE_p_iDynTree__UnknownWrenchContact swig_types[174]
+#define SWIGTYPE_p_iDynTree__VectorDynSize swig_types[175]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_10_t swig_types[176]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_16_t swig_types[177]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_2_t swig_types[178]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t swig_types[179]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t swig_types[180]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t swig_types[181]
+#define SWIGTYPE_p_iDynTree__Visualizer swig_types[182]
+#define SWIGTYPE_p_iDynTree__VisualizerOptions swig_types[183]
+#define SWIGTYPE_p_iDynTree__Wrench swig_types[184]
+#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t swig_types[185]
+#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t swig_types[186]
+#define SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers swig_types[187]
+#define SWIGTYPE_p_index_type swig_types[188]
+#define SWIGTYPE_p_int swig_types[189]
+#define SWIGTYPE_p_iterator swig_types[190]
+#define SWIGTYPE_p_pointer swig_types[191]
+#define SWIGTYPE_p_reference swig_types[192]
+#define SWIGTYPE_p_reverse_iterator swig_types[193]
+#define SWIGTYPE_p_size_type swig_types[194]
+#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t swig_types[195]
+#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdySensor_t swig_types[196]
+#define SWIGTYPE_p_std__allocatorT_std__string_t swig_types[197]
+#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t swig_types[198]
+#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t swig_types[199]
+#define SWIGTYPE_p_std__string swig_types[200]
+#define SWIGTYPE_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t swig_types[201]
+#define SWIGTYPE_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t swig_types[202]
+#define SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t swig_types[203]
+#define SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t swig_types[204]
+#define SWIGTYPE_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t swig_types[205]
+#define SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t swig_types[206]
+#define SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t swig_types[207]
+#define SWIGTYPE_p_std__vectorT_int_std__allocatorT_int_t_t swig_types[208]
+#define SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t swig_types[209]
+#define SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t swig_types[210]
+#define SWIGTYPE_p_swig__MatlabSwigIterator swig_types[211]
+#define SWIGTYPE_p_typed_iterator swig_types[212]
+#define SWIGTYPE_p_unsigned_int swig_types[213]
+#define SWIGTYPE_p_unsigned_long swig_types[214]
+#define SWIGTYPE_p_value_type swig_types[215]
+static swig_type_info *swig_types[217];
+static swig_module_info swig_module = {swig_types, 216, 0, 0, 0, 0};
 #define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name)
 #define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name)
 
@@ -2978,6 +2982,7 @@ SWIGINTERN void std_vector_Sl_std_string_Sg__insert__SWIG_1(std::vector< std::st
 #include "iDynTree/ModelIO/URDFDofsImport.h"
 #include "iDynTree/ModelIO/URDFGenericSensorsImport.h"
 #include "iDynTree/ModelIO/ModelLoader.h"
+#include "iDynTree/ModelIO/ModelExporter.h"
 
 // Estimation related classes
 #include "iDynTree/Estimation/ExternalWrenchesEstimation.h"
@@ -2990,6 +2995,9 @@ SWIGINTERN void std_vector_Sl_std_string_Sg__insert__SWIG_1(std::vector< std::st
 #include "iDynTree/Estimation/ExtendedKalmanFilter.h"
 #include "iDynTree/Estimation/AttitudeQuaternionEKF.h"
 
+// SolidShapes related classes
+#include "iDynTree/InertialParametersSolidShapesHelpers.h"
+
 // Regressors related data structures
 #include "iDynTree/Regressors/DynamicsRegressorParameters.h"
 #include "iDynTree/Regressors/DynamicsRegressorGenerator.h"
@@ -3159,7 +3167,7 @@ SWIGINTERN void iDynTree_MatrixDynSize_fromMatlab(iDynTree::MatrixDynSize *self,
 
         if (mxIsSparse(in))
         {
-            /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+            /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3425,7 +3433,7 @@ SWIGINTERN void iDynTree_VectorDynSize_fromMatlab(iDynTree::VectorDynSize *self,
 
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3487,7 +3495,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_1_Sc_6_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3550,7 +3558,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_2_Sc_3_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3613,7 +3621,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_3_Sc_3_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3676,7 +3684,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_4_Sc_4_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3739,7 +3747,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_6_Sc_6_Sg__fromMatlab(iDynTree::Matrix
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3802,7 +3810,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_6_Sc_10_Sg__fromMatlab(iDynTree::Matri
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3865,7 +3873,7 @@ SWIGINTERN void iDynTree_MatrixFixSize_Sl_10_Sc_16_Sg__fromMatlab(iDynTree::Matr
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,43,MATRIXSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3929,7 +3937,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_3_Sg__fromMatlab(iDynTree::VectorFixSi
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -3995,7 +4003,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_4_Sg__fromMatlab(iDynTree::VectorFixSi
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -4061,7 +4069,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_6_Sg__fromMatlab(iDynTree::VectorFixSi
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -4127,7 +4135,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_10_Sg__fromMatlab(iDynTree::VectorFixS
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -4193,7 +4201,7 @@ SWIGINTERN void iDynTree_VectorFixSize_Sl_16_Sg__fromMatlab(iDynTree::VectorFixS
         {
             if (mxIsSparse(in))
             {
-                /*@SWIG:/home/pramadoss/iit_ws/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
+                /*@SWIG:/home/straversaro/src/robotology-superbuild/robotology/iDynTree/bindings/./matlab/matlab_matvec.i,6,VECTORSPARSECOPY@*/
     //getting pointer to sparse structure
     mwIndex* ir = mxGetIr(in);
     mwIndex* jc = mxGetJc(in);
@@ -38291,6 +38299,70 @@ int _wrap_Rotation_RPYRightTrivializedDerivative(int resc, mxArray *resv[], int
 }
 
 
+int _wrap_Rotation_RPYRightTrivializedDerivativeRateOfChange(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  double arg1 ;
+  double arg2 ;
+  double arg3 ;
+  double arg4 ;
+  double arg5 ;
+  double arg6 ;
+  double val1 ;
+  int ecode1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
+  double val4 ;
+  int ecode4 = 0 ;
+  double val5 ;
+  int ecode5 = 0 ;
+  double val6 ;
+  int ecode6 = 0 ;
+  mxArray * _out;
+  iDynTree::Matrix3x3 result;
+  
+  if (!SWIG_check_num_args("Rotation_RPYRightTrivializedDerivativeRateOfChange",argc,6,6,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_double(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "Rotation_RPYRightTrivializedDerivativeRateOfChange" "', argument " "1"" of type '" "double""'");
+  } 
+  arg1 = static_cast< double >(val1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Rotation_RPYRightTrivializedDerivativeRateOfChange" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Rotation_RPYRightTrivializedDerivativeRateOfChange" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  ecode4 = SWIG_AsVal_double(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "Rotation_RPYRightTrivializedDerivativeRateOfChange" "', argument " "4"" of type '" "double""'");
+  } 
+  arg4 = static_cast< double >(val4);
+  ecode5 = SWIG_AsVal_double(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "Rotation_RPYRightTrivializedDerivativeRateOfChange" "', argument " "5"" of type '" "double""'");
+  } 
+  arg5 = static_cast< double >(val5);
+  ecode6 = SWIG_AsVal_double(argv[5], &val6);
+  if (!SWIG_IsOK(ecode6)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "Rotation_RPYRightTrivializedDerivativeRateOfChange" "', argument " "6"" of type '" "double""'");
+  } 
+  arg6 = static_cast< double >(val6);
+  result = iDynTree::Rotation::RPYRightTrivializedDerivativeRateOfChange(arg1,arg2,arg3,arg4,arg5,arg6);
+  _out = SWIG_NewPointerObj((new iDynTree::Matrix3x3(static_cast< const iDynTree::Matrix3x3& >(result))), SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_3_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_Rotation_RPYRightTrivializedDerivativeInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   double arg1 ;
   double arg2 ;
@@ -38331,6 +38403,70 @@ int _wrap_Rotation_RPYRightTrivializedDerivativeInverse(int resc, mxArray *resv[
 }
 
 
+int _wrap_Rotation_RPYRightTrivializedDerivativeInverseRateOfChange(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  double arg1 ;
+  double arg2 ;
+  double arg3 ;
+  double arg4 ;
+  double arg5 ;
+  double arg6 ;
+  double val1 ;
+  int ecode1 = 0 ;
+  double val2 ;
+  int ecode2 = 0 ;
+  double val3 ;
+  int ecode3 = 0 ;
+  double val4 ;
+  int ecode4 = 0 ;
+  double val5 ;
+  int ecode5 = 0 ;
+  double val6 ;
+  int ecode6 = 0 ;
+  mxArray * _out;
+  iDynTree::Matrix3x3 result;
+  
+  if (!SWIG_check_num_args("Rotation_RPYRightTrivializedDerivativeInverseRateOfChange",argc,6,6,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_double(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange" "', argument " "1"" of type '" "double""'");
+  } 
+  arg1 = static_cast< double >(val1);
+  ecode2 = SWIG_AsVal_double(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange" "', argument " "2"" of type '" "double""'");
+  } 
+  arg2 = static_cast< double >(val2);
+  ecode3 = SWIG_AsVal_double(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange" "', argument " "3"" of type '" "double""'");
+  } 
+  arg3 = static_cast< double >(val3);
+  ecode4 = SWIG_AsVal_double(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange" "', argument " "4"" of type '" "double""'");
+  } 
+  arg4 = static_cast< double >(val4);
+  ecode5 = SWIG_AsVal_double(argv[4], &val5);
+  if (!SWIG_IsOK(ecode5)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode5), "in method '" "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange" "', argument " "5"" of type '" "double""'");
+  } 
+  arg5 = static_cast< double >(val5);
+  ecode6 = SWIG_AsVal_double(argv[5], &val6);
+  if (!SWIG_IsOK(ecode6)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode6), "in method '" "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange" "', argument " "6"" of type '" "double""'");
+  } 
+  arg6 = static_cast< double >(val6);
+  result = iDynTree::Rotation::RPYRightTrivializedDerivativeInverseRateOfChange(arg1,arg2,arg3,arg4,arg5,arg6);
+  _out = SWIG_NewPointerObj((new iDynTree::Matrix3x3(static_cast< const iDynTree::Matrix3x3& >(result))), SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_3_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_Rotation_QuaternionRightTrivializedDerivative(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Vector4 arg1 ;
   void *argp1 ;
@@ -38435,6 +38571,60 @@ int _wrap_Rotation_RotationFromQuaternion(int resc, mxArray *resv[], int argc, m
 }
 
 
+int _wrap_Rotation_leftJacobian(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AngularMotionVector3 *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Matrix3x3 result;
+  
+  if (!SWIG_check_num_args("Rotation_leftJacobian",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__AngularMotionVector3,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Rotation_leftJacobian" "', argument " "1"" of type '" "iDynTree::AngularMotionVector3 const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Rotation_leftJacobian" "', argument " "1"" of type '" "iDynTree::AngularMotionVector3 const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AngularMotionVector3 * >(argp1);
+  result = iDynTree::Rotation::leftJacobian((iDynTree::AngularMotionVector3 const &)*arg1);
+  _out = SWIG_NewPointerObj((new iDynTree::Matrix3x3(static_cast< const iDynTree::Matrix3x3& >(result))), SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_3_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_Rotation_leftJacobianInverse(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AngularMotionVector3 *arg1 = 0 ;
+  void *argp1 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Matrix3x3 result;
+  
+  if (!SWIG_check_num_args("Rotation_leftJacobianInverse",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1, SWIGTYPE_p_iDynTree__AngularMotionVector3,  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Rotation_leftJacobianInverse" "', argument " "1"" of type '" "iDynTree::AngularMotionVector3 const &""'"); 
+  }
+  if (!argp1) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Rotation_leftJacobianInverse" "', argument " "1"" of type '" "iDynTree::AngularMotionVector3 const &""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AngularMotionVector3 * >(argp1);
+  result = iDynTree::Rotation::leftJacobianInverse((iDynTree::AngularMotionVector3 const &)*arg1);
+  _out = SWIG_NewPointerObj((new iDynTree::Matrix3x3(static_cast< const iDynTree::Matrix3x3& >(result))), SWIGTYPE_p_iDynTree__MatrixFixSizeT_3_3_t, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_Rotation_toString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Rotation *arg1 = (iDynTree::Rotation *) 0 ;
   void *argp1 = 0 ;
@@ -52845,6 +53035,61 @@ int _wrap_SolidShape_name_get(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
+int _wrap_SolidShape_nameIsValid_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  bool arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("SolidShape_nameIsValid_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_nameIsValid_set" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SolidShape_nameIsValid_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->nameIsValid = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_SolidShape_nameIsValid_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("SolidShape_nameIsValid_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SolidShape, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SolidShape_nameIsValid_get" "', argument " "1"" of type '" "iDynTree::SolidShape *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SolidShape * >(argp1);
+  result = (bool) ((arg1)->nameIsValid);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_SolidShape_link_H_geometry_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::SolidShape *arg1 = (iDynTree::SolidShape *) 0 ;
   iDynTree::Transform *arg2 = (iDynTree::Transform *) 0 ;
@@ -54992,6 +55237,30 @@ int _wrap_Model_getJointName(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
+int _wrap_Model_getTotalMass(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  double result;
+  
+  if (!SWIG_check_num_args("Model_getTotalMass",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getTotalMass" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  result = (double)((iDynTree::Model const *)arg1)->getTotalMass();
+  _out = SWIG_From_double(static_cast< double >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_Model_getJointIndex(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
   std::string *arg2 = 0 ;
@@ -55956,6 +56225,49 @@ int _wrap_Model_getFrameLink(int resc, mxArray *resv[], int argc, mxArray *argv[
 }
 
 
+int _wrap_Model_getLinkAdditionalFrames(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
+  iDynTree::LinkIndex arg2 ;
+  std::vector< iDynTree::FrameIndex,std::allocator< iDynTree::FrameIndex > > *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("Model_getLinkAdditionalFrames",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Model_getLinkAdditionalFrames" "', argument " "1"" of type '" "iDynTree::Model const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::Model * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Model_getLinkAdditionalFrames" "', argument " "2"" of type '" "iDynTree::LinkIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::LinkIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_std__vectorT_int_std__allocatorT_int_t_t,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "Model_getLinkAdditionalFrames" "', argument " "3"" of type '" "std::vector< iDynTree::FrameIndex,std::allocator< iDynTree::FrameIndex > > &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "Model_getLinkAdditionalFrames" "', argument " "3"" of type '" "std::vector< iDynTree::FrameIndex,std::allocator< iDynTree::FrameIndex > > &""'"); 
+  }
+  arg3 = reinterpret_cast< std::vector< iDynTree::FrameIndex,std::allocator< iDynTree::FrameIndex > > * >(argp3);
+  result = (bool)((iDynTree::Model const *)arg1)->getLinkAdditionalFrames(arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_Model_getNrOfNeighbors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Model *arg1 = (iDynTree::Model *) 0 ;
   iDynTree::LinkIndex arg2 ;
@@ -70181,6 +70493,800 @@ int _wrap_ModelLoader_isValid(int resc, mxArray *resv[], int argc, mxArray *argv
 }
 
 
+int _wrap_ModelExporterOptions_baseLink_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporterOptions *arg1 = (iDynTree::ModelExporterOptions *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ModelExporterOptions_baseLink_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporterOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporterOptions_baseLink_set" "', argument " "1"" of type '" "iDynTree::ModelExporterOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporterOptions * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporterOptions_baseLink_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporterOptions_baseLink_set" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  if (arg1) (arg1)->baseLink = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelExporterOptions_baseLink_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporterOptions *arg1 = (iDynTree::ModelExporterOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  std::string *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelExporterOptions_baseLink_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporterOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporterOptions_baseLink_get" "', argument " "1"" of type '" "iDynTree::ModelExporterOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporterOptions * >(argp1);
+  result = (std::string *) & ((arg1)->baseLink);
+  _out = SWIG_From_std_string(static_cast< std::string >(*result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporterOptions *arg1 = (iDynTree::ModelExporterOptions *) 0 ;
+  bool arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  bool val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporterOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_set" "', argument " "1"" of type '" "iDynTree::ModelExporterOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporterOptions * >(argp1);
+  ecode2 = SWIG_AsVal_bool(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_set" "', argument " "2"" of type '" "bool""'");
+  } 
+  arg2 = static_cast< bool >(val2);
+  if (arg1) (arg1)->exportFirstBaseLinkAdditionalFrameAsFakeURDFBase = arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporterOptions *arg1 = (iDynTree::ModelExporterOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporterOptions, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_get" "', argument " "1"" of type '" "iDynTree::ModelExporterOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporterOptions * >(argp1);
+  result = (bool) ((arg1)->exportFirstBaseLinkAdditionalFrameAsFakeURDFBase);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_ModelExporterOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::ModelExporterOptions *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_ModelExporterOptions",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::ModelExporterOptions *)new iDynTree::ModelExporterOptions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelExporterOptions, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_ModelExporterOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporterOptions *arg1 = (iDynTree::ModelExporterOptions *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ModelExporterOptions",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporterOptions, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelExporterOptions" "', argument " "1"" of type '" "iDynTree::ModelExporterOptions *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporterOptions * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_ModelExporter(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::ModelExporter *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_ModelExporter",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::ModelExporter *)new iDynTree::ModelExporter();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelExporter, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_ModelExporter(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ModelExporter",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelExporter" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportingOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::ModelExporterOptions *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelExporter_exportingOptions",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_exportingOptions" "', argument " "1"" of type '" "iDynTree::ModelExporter const *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  result = (iDynTree::ModelExporterOptions *) &((iDynTree::ModelExporter const *)arg1)->exportingOptions();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelExporterOptions, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_setExportingOptions(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  iDynTree::ModelExporterOptions *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("ModelExporter_setExportingOptions",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_setExportingOptions" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__ModelExporterOptions,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_setExportingOptions" "', argument " "2"" of type '" "iDynTree::ModelExporterOptions const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_setExportingOptions" "', argument " "2"" of type '" "iDynTree::ModelExporterOptions const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::ModelExporterOptions * >(argp2);
+  (arg1)->setExportingOptions((iDynTree::ModelExporterOptions const &)*arg2);
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_init__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
+  iDynTree::ModelExporterOptions arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp4 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_init",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_init" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelExporter_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  {
+    res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__ModelExporterOptions,  0 );
+    if (!SWIG_IsOK(res4)) {
+      SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "ModelExporter_init" "', argument " "4"" of type '" "iDynTree::ModelExporterOptions const""'"); 
+    }  
+    if (!argp4) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_init" "', argument " "4"" of type '" "iDynTree::ModelExporterOptions const""'");
+    } else {
+      arg4 = *(reinterpret_cast< iDynTree::ModelExporterOptions * >(argp4));
+    }
+  }
+  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_init__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::SensorsList *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_init",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_init" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__SensorsList,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelExporter_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_init" "', argument " "3"" of type '" "iDynTree::SensorsList const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::SensorsList * >(argp3);
+  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2,(iDynTree::SensorsList const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_init__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  iDynTree::Model *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_init",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_init" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_init" "', argument " "2"" of type '" "iDynTree::Model const &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  result = (bool)(arg1)->init((iDynTree::Model const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_init(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelExporter_init__SWIG_2(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelExporter_init__SWIG_1(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__Model, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__SensorsList, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__ModelExporterOptions, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ModelExporter_init__SWIG_0(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelExporter_init'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelExporter::init(iDynTree::Model const &,iDynTree::SensorsList const &,iDynTree::ModelExporterOptions const)\n"
+    "    iDynTree::ModelExporter::init(iDynTree::Model const &,iDynTree::SensorsList const &)\n"
+    "    iDynTree::ModelExporter::init(iDynTree::Model const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelExporter_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Model *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelExporter_model",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_model" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  result = (iDynTree::Model *) &(arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::SensorsList *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelExporter_sensors",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_sensors" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  result = (iDynTree::SensorsList *) &(arg1)->sensors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_isValid",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_isValid" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  result = (bool)(arg1)->isValid();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportModelToString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_exportModelToString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_exportModelToString" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__string,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_exportModelToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_exportModelToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  arg2 = reinterpret_cast< std::string * >(argp2);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelExporter_exportModelToString" "', argument " "3"" of type '" "std::string const""'"); 
+    }
+    arg3 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->exportModelToString(*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportModelToString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_exportModelToString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_exportModelToString" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__string,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_exportModelToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_exportModelToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  arg2 = reinterpret_cast< std::string * >(argp2);
+  result = (bool)(arg1)->exportModelToString(*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportModelToString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_std__string, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelExporter_exportModelToString__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_std__string, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelExporter_exportModelToString__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelExporter_exportModelToString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelExporter::exportModelToString(std::string &,std::string const)\n"
+    "    iDynTree::ModelExporter::exportModelToString(std::string &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportModelToFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string arg3 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_exportModelToFile",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_exportModelToFile" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_exportModelToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_exportModelToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelExporter_exportModelToFile" "', argument " "3"" of type '" "std::string const""'"); 
+    }
+    arg3 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->exportModelToFile((std::string const &)*arg2,arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportModelToFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelExporter *arg1 = (iDynTree::ModelExporter *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelExporter_exportModelToFile",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelExporter, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelExporter_exportModelToFile" "', argument " "1"" of type '" "iDynTree::ModelExporter *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelExporter * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelExporter_exportModelToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelExporter_exportModelToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->exportModelToFile((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelExporter_exportModelToFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelExporter_exportModelToFile__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelExporter, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelExporter_exportModelToFile__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelExporter_exportModelToFile'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelExporter::exportModelToFile(std::string const &,std::string const)\n"
+    "    iDynTree::ModelExporter::exportModelToFile(std::string const &)\n");
+  return 1;
+}
+
+
 int _wrap_new_UnknownWrenchContact__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
   iDynTree::UnknownWrenchContact *result = 0 ;
@@ -74545,6 +75651,99 @@ int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_1(int resc, mxArray *resv[
 }
 
 
+int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::Transform *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_changeFixedFrame",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  result = (bool)(arg1)->changeFixedFrame((std::string const &)*arg2,(iDynTree::Transform const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_3(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  iDynTree::Transform *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_changeFixedFrame",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__Transform,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "SimpleLeggedOdometry_changeFixedFrame" "', argument " "3"" of type '" "iDynTree::Transform const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::Transform * >(argp3);
+  result = (bool)(arg1)->changeFixedFrame(arg2,(iDynTree::Transform const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_SimpleLeggedOdometry_changeFixedFrame(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   if (argc == 2) {
     int _v;
@@ -74574,11 +75773,51 @@ int _wrap_SimpleLeggedOdometry_changeFixedFrame(int resc, mxArray *resv[], int a
       }
     }
   }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_int(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_3(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__Transform, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_SimpleLeggedOdometry_changeFixedFrame__SWIG_2(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
   
   SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'SimpleLeggedOdometry_changeFixedFrame'."
     "  Possible C/C++ prototypes are:\n"
     "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(std::string const &)\n"
-    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(iDynTree::FrameIndex const)\n");
+    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(iDynTree::FrameIndex const)\n"
+    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(std::string const &,iDynTree::Transform const &)\n"
+    "    iDynTree::SimpleLeggedOdometry::changeFixedFrame(iDynTree::FrameIndex const,iDynTree::Transform const &)\n");
   return 1;
 }
 
@@ -74639,6 +75878,38 @@ int _wrap_SimpleLeggedOdometry_getWorldLinkTransform(int resc, mxArray *resv[],
 }
 
 
+int _wrap_SimpleLeggedOdometry_getWorldFrameTransform(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::SimpleLeggedOdometry *arg1 = (iDynTree::SimpleLeggedOdometry *) 0 ;
+  iDynTree::FrameIndex arg2 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int val2 ;
+  int ecode2 = 0 ;
+  mxArray * _out;
+  iDynTree::Transform result;
+  
+  if (!SWIG_check_num_args("SimpleLeggedOdometry_getWorldFrameTransform",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__SimpleLeggedOdometry, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "SimpleLeggedOdometry_getWorldFrameTransform" "', argument " "1"" of type '" "iDynTree::SimpleLeggedOdometry *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::SimpleLeggedOdometry * >(argp1);
+  ecode2 = SWIG_AsVal_int(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "SimpleLeggedOdometry_getWorldFrameTransform" "', argument " "2"" of type '" "iDynTree::FrameIndex""'");
+  } 
+  arg2 = static_cast< iDynTree::FrameIndex >(val2);
+  result = (arg1)->getWorldFrameTransform(arg2);
+  _out = SWIG_NewPointerObj((new iDynTree::Transform(static_cast< const iDynTree::Transform& >(result))), SWIGTYPE_p_iDynTree__Transform, SWIG_POINTER_OWN |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_isLinkBerdyDynamicVariable(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::BerdyDynamicVariablesTypes arg1 ;
   int val1 ;
@@ -78468,6 +79739,215 @@ int _wrap_BerdySparseMAPSolver_getLastEstimate(int resc, mxArray *resv[], int ar
 }
 
 
+int _wrap_AttitudeEstimatorState_m_orientation_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  iDynTree::UnitQuaternion *arg2 = (iDynTree::UnitQuaternion *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_orientation_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_orientation_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_orientation_set" "', argument " "2"" of type '" "iDynTree::UnitQuaternion *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::UnitQuaternion * >(argp2);
+  if (arg1) (arg1)->m_orientation = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeEstimatorState_m_orientation_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::UnitQuaternion *result = 0 ;
+  
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_orientation_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_orientation_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  result = (iDynTree::UnitQuaternion *)& ((arg1)->m_orientation);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeEstimatorState_m_angular_velocity_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_angular_velocity_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_angular_velocity_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_angular_velocity_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
+  if (arg1) (arg1)->m_angular_velocity = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeEstimatorState_m_angular_velocity_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Vector3 *result = 0 ;
+  
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_angular_velocity_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_angular_velocity_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  result = (iDynTree::Vector3 *)& ((arg1)->m_angular_velocity);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeEstimatorState_m_gyroscope_bias_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  mxArray * _out;
+  
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_gyroscope_bias_set",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
+  if (arg1) (arg1)->m_gyroscope_bias = *arg2;
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_AttitudeEstimatorState_m_gyroscope_bias_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Vector3 *result = 0 ;
+  
+  if (!SWIG_check_num_args("AttitudeEstimatorState_m_gyroscope_bias_get",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  result = (iDynTree::Vector3 *)& ((arg1)->m_gyroscope_bias);
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_new_AttitudeEstimatorState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::AttitudeEstimatorState *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_AttitudeEstimatorState",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::AttitudeEstimatorState *)new iDynTree::AttitudeEstimatorState();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_AttitudeEstimatorState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_AttitudeEstimatorState",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeEstimatorState" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_delete_IAttitudeEstimator(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::IAttitudeEstimator *arg1 = (iDynTree::IAttitudeEstimator *) 0 ;
   void *argp1 = 0 ;
@@ -80117,7 +81597,7 @@ int _wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(int resc, mxArray *resv[], in
 }
 
 
-int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
   iDynTree::MatrixDynSize *arg3 = 0 ;
@@ -80163,6 +81643,116 @@ int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(int resc, mxArr
 }
 
 
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  iDynTree::VectorDynSize *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  iDynTree::MatrixDynSize *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  void *argp4 = 0 ;
+  int res4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "2"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::VectorDynSize * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  res4 = SWIG_ConvertPtr(argv[3], &argp4, SWIGTYPE_p_iDynTree__MatrixDynSize,  0 );
+  if (!SWIG_IsOK(res4)) {
+    SWIG_exception_fail(SWIG_ArgError(res4), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  if (!argp4) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF" "', argument " "4"" of type '" "iDynTree::MatrixDynSize &""'"); 
+  }
+  arg4 = reinterpret_cast< iDynTree::MatrixDynSize * >(argp4);
+  result = (bool)(arg1)->ekfComputeJacobianF(*arg2,*arg3,*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          void *vptr = 0;
+          int res = SWIG_ConvertPtr(argv[3], &vptr, SWIGTYPE_p_iDynTree__MatrixDynSize, 0);
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::DiscreteExtendedKalmanFilterHelper::ekfComputeJacobianF(iDynTree::VectorDynSize &,iDynTree::MatrixDynSize &)\n"
+    "    iDynTree::DiscreteExtendedKalmanFilterHelper::ekfComputeJacobianF(iDynTree::VectorDynSize &,iDynTree::VectorDynSize &,iDynTree::MatrixDynSize &)\n");
+  return 1;
+}
+
+
 int _wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   iDynTree::VectorDynSize *arg2 = 0 ;
@@ -80257,7 +81847,7 @@ int _wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(int resc, mxArray *resv[]
 }
 
 
-int _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -80281,7 +81871,107 @@ int _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(int resc, mxArray *resv[],
 }
 
 
-int _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  size_t *arg2 = 0 ;
+  size_t *arg3 = 0 ;
+  size_t *arg4 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  size_t temp2 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  size_t temp3 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  size_t temp4 ;
+  size_t val4 ;
+  int ecode4 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfInit",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfInit" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfInit" "', argument " "2"" of type '" "size_t""'");
+  } 
+  temp2 = static_cast< size_t >(val2);
+  arg2 = &temp2;
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfInit" "', argument " "3"" of type '" "size_t""'");
+  } 
+  temp3 = static_cast< size_t >(val3);
+  arg3 = &temp3;
+  ecode4 = SWIG_AsVal_size_t(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfInit" "', argument " "4"" of type '" "size_t""'");
+  } 
+  temp4 = static_cast< size_t >(val4);
+  arg4 = &temp4;
+  result = (bool)(arg1)->ekfInit((size_t const &)*arg2,(size_t const &)*arg3,(size_t const &)*arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          {
+            int res = SWIG_AsVal_size_t(argv[3], NULL);
+            _v = SWIG_CheckState(res);
+          }
+          if (_v) {
+            return _wrap_DiscreteExtendedKalmanFilterHelper_ekfInit__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DiscreteExtendedKalmanFilterHelper_ekfInit'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit()\n"
+    "    iDynTree::DiscreteExtendedKalmanFilterHelper::ekfInit(size_t const &,size_t const &,size_t const &)\n");
+  return 1;
+}
+
+
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   void *argp1 = 0 ;
   int res1 = 0 ;
@@ -80304,6 +81994,170 @@ int _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(int resc, mxArray *resv[],
 }
 
 
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
+  size_t *arg2 = 0 ;
+  size_t *arg3 = 0 ;
+  size_t *arg4 = 0 ;
+  iDynTree::Span< double,-1 > *arg5 = 0 ;
+  iDynTree::Span< double,-1 > *arg6 = 0 ;
+  iDynTree::Span< double,-1 > *arg7 = 0 ;
+  iDynTree::Span< double,-1 > *arg8 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  size_t temp2 ;
+  size_t val2 ;
+  int ecode2 = 0 ;
+  size_t temp3 ;
+  size_t val3 ;
+  int ecode3 = 0 ;
+  size_t temp4 ;
+  size_t val4 ;
+  int ecode4 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  void *argp6 ;
+  int res6 = 0 ;
+  void *argp7 ;
+  int res7 = 0 ;
+  void *argp8 ;
+  int res8 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("DiscreteExtendedKalmanFilterHelper_ekfReset",argc,8,8,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "1"" of type '" "iDynTree::DiscreteExtendedKalmanFilterHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::DiscreteExtendedKalmanFilterHelper * >(argp1);
+  ecode2 = SWIG_AsVal_size_t(argv[1], &val2);
+  if (!SWIG_IsOK(ecode2)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "2"" of type '" "size_t""'");
+  } 
+  temp2 = static_cast< size_t >(val2);
+  arg2 = &temp2;
+  ecode3 = SWIG_AsVal_size_t(argv[2], &val3);
+  if (!SWIG_IsOK(ecode3)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode3), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "3"" of type '" "size_t""'");
+  } 
+  temp3 = static_cast< size_t >(val3);
+  arg3 = &temp3;
+  ecode4 = SWIG_AsVal_size_t(argv[3], &val4);
+  if (!SWIG_IsOK(ecode4)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode4), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "4"" of type '" "size_t""'");
+  } 
+  temp4 = static_cast< size_t >(val4);
+  arg4 = &temp4;
+  res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res5)) {
+    SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "5"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp5) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "5"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg5 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp5);
+  res6 = SWIG_ConvertPtr(argv[5], &argp6, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res6)) {
+    SWIG_exception_fail(SWIG_ArgError(res6), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "6"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp6) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "6"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg6 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp6);
+  res7 = SWIG_ConvertPtr(argv[6], &argp7, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res7)) {
+    SWIG_exception_fail(SWIG_ArgError(res7), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "7"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp7) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "7"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg7 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp7);
+  res8 = SWIG_ConvertPtr(argv[7], &argp8, SWIGTYPE_p_iDynTree__SpanT_double__1_t,  0 );
+  if (!SWIG_IsOK(res8)) {
+    SWIG_exception_fail(SWIG_ArgError(res8), "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "8"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  if (!argp8) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "DiscreteExtendedKalmanFilterHelper_ekfReset" "', argument " "8"" of type '" "iDynTree::Span< double,-1 > const &""'"); 
+  }
+  arg8 = reinterpret_cast< iDynTree::Span< double,-1 > * >(argp8);
+  result = (bool)(arg1)->ekfReset((size_t const &)*arg2,(size_t const &)*arg3,(size_t const &)*arg4,(iDynTree::Span< double,-1 > const &)*arg5,(iDynTree::Span< double,-1 > const &)*arg6,(iDynTree::Span< double,-1 > const &)*arg7,(iDynTree::Span< double,-1 > const &)*arg8);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 1) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      return _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset__SWIG_0(resc,resv,argc,argv);
+    }
+  }
+  if (argc == 8) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__DiscreteExtendedKalmanFilterHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      {
+        int res = SWIG_AsVal_size_t(argv[1], NULL);
+        _v = SWIG_CheckState(res);
+      }
+      if (_v) {
+        {
+          int res = SWIG_AsVal_size_t(argv[2], NULL);
+          _v = SWIG_CheckState(res);
+        }
+        if (_v) {
+          {
+            int res = SWIG_AsVal_size_t(argv[3], NULL);
+            _v = SWIG_CheckState(res);
+          }
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              void *vptr = 0;
+              int res = SWIG_ConvertPtr(argv[5], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
+              _v = SWIG_CheckState(res);
+              if (_v) {
+                void *vptr = 0;
+                int res = SWIG_ConvertPtr(argv[6], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
+                _v = SWIG_CheckState(res);
+                if (_v) {
+                  void *vptr = 0;
+                  int res = SWIG_ConvertPtr(argv[7], &vptr, SWIGTYPE_p_iDynTree__SpanT_double__1_t, 0);
+                  _v = SWIG_CheckState(res);
+                  if (_v) {
+                    return _wrap_DiscreteExtendedKalmanFilterHelper_ekfReset__SWIG_1(resc,resv,argc,argv);
+                  }
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'DiscreteExtendedKalmanFilterHelper_ekfReset'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset()\n"
+    "    iDynTree::DiscreteExtendedKalmanFilterHelper::ekfReset(size_t const &,size_t const &,size_t const &,iDynTree::Span< double,-1 > const &,iDynTree::Span< double,-1 > const &,iDynTree::Span< double,-1 > const &,iDynTree::Span< double,-1 > const &)\n");
+  return 1;
+}
+
+
 int _wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::DiscreteExtendedKalmanFilterHelper *arg1 = (iDynTree::DiscreteExtendedKalmanFilterHelper *) 0 ;
   iDynTree::Span< double,-1 > *arg2 = 0 ;
@@ -80722,215 +82576,6 @@ SWIGINTERN int _wrap_input_dimensions_get(int resc, mxArray *resv[], int argc, m
 }
 
 
-int _wrap_AttitudeEstimatorState_m_orientation_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  iDynTree::UnitQuaternion *arg2 = (iDynTree::UnitQuaternion *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("AttitudeEstimatorState_m_orientation_set",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_orientation_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_orientation_set" "', argument " "2"" of type '" "iDynTree::UnitQuaternion *""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::UnitQuaternion * >(argp2);
-  if (arg1) (arg1)->m_orientation = *arg2;
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_AttitudeEstimatorState_m_orientation_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::UnitQuaternion *result = 0 ;
-  
-  if (!SWIG_check_num_args("AttitudeEstimatorState_m_orientation_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_orientation_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  result = (iDynTree::UnitQuaternion *)& ((arg1)->m_orientation);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_AttitudeEstimatorState_m_angular_velocity_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("AttitudeEstimatorState_m_angular_velocity_set",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_angular_velocity_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_angular_velocity_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
-  if (arg1) (arg1)->m_angular_velocity = *arg2;
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_AttitudeEstimatorState_m_angular_velocity_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::Vector3 *result = 0 ;
-  
-  if (!SWIG_check_num_args("AttitudeEstimatorState_m_angular_velocity_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_angular_velocity_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  result = (iDynTree::Vector3 *)& ((arg1)->m_angular_velocity);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_AttitudeEstimatorState_m_gyroscope_bias_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  iDynTree::Vector3 *arg2 = (iDynTree::Vector3 *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  void *argp2 = 0 ;
-  int res2 = 0 ;
-  mxArray * _out;
-  
-  if (!SWIG_check_num_args("AttitudeEstimatorState_m_gyroscope_bias_set",argc,2,2,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_set" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  res2 = SWIG_ConvertPtr(argv[1], &argp2,SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
-  if (!SWIG_IsOK(res2)) {
-    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_set" "', argument " "2"" of type '" "iDynTree::Vector3 *""'"); 
-  }
-  arg2 = reinterpret_cast< iDynTree::Vector3 * >(argp2);
-  if (arg1) (arg1)->m_gyroscope_bias = *arg2;
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_AttitudeEstimatorState_m_gyroscope_bias_get(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  iDynTree::Vector3 *result = 0 ;
-  
-  if (!SWIG_check_num_args("AttitudeEstimatorState_m_gyroscope_bias_get",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 0 |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "AttitudeEstimatorState_m_gyroscope_bias_get" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  result = (iDynTree::Vector3 *)& ((arg1)->m_gyroscope_bias);
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t, 0 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_new_AttitudeEstimatorState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  mxArray * _out;
-  iDynTree::AttitudeEstimatorState *result = 0 ;
-  
-  if (!SWIG_check_num_args("new_AttitudeEstimatorState",argc,0,0,0)) {
-    SWIG_fail;
-  }
-  (void)argv;
-  result = (iDynTree::AttitudeEstimatorState *)new iDynTree::AttitudeEstimatorState();
-  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__AttitudeEstimatorState, 1 |  0 );
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
-int _wrap_delete_AttitudeEstimatorState(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
-  iDynTree::AttitudeEstimatorState *arg1 = (iDynTree::AttitudeEstimatorState *) 0 ;
-  void *argp1 = 0 ;
-  int res1 = 0 ;
-  mxArray * _out;
-  
-  int is_owned;
-  if (!SWIG_check_num_args("delete_AttitudeEstimatorState",argc,1,1,0)) {
-    SWIG_fail;
-  }
-  is_owned = SWIG_Matlab_isOwned(argv[0]);
-  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__AttitudeEstimatorState, SWIG_POINTER_DISOWN |  0 );
-  if (!SWIG_IsOK(res1)) {
-    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_AttitudeEstimatorState" "', argument " "1"" of type '" "iDynTree::AttitudeEstimatorState *""'"); 
-  }
-  arg1 = reinterpret_cast< iDynTree::AttitudeEstimatorState * >(argp1);
-  if (is_owned) {
-    delete arg1;
-  }
-  _out = (mxArray*)0;
-  if (_out) --resc, *resv++ = _out;
-  return 0;
-fail:
-  return 1;
-}
-
-
 int _wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::AttitudeQuaternionEKFParameters *arg1 = (iDynTree::AttitudeQuaternionEKFParameters *) 0 ;
   double arg2 ;
@@ -82366,6 +84011,52 @@ int _wrap_delete_AttitudeQuaternionEKF(int resc, mxArray *resv[], int argc, mxAr
 }
 
 
+int _wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  double arg1 ;
+  iDynTree::Model *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  double val1 ;
+  int ecode1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 = 0 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("estimateInertialParametersFromLinkBoundingBoxesAndTotalMass",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  ecode1 = SWIG_AsVal_double(argv[0], &val1);
+  if (!SWIG_IsOK(ecode1)) {
+    SWIG_exception_fail(SWIG_ArgError(ecode1), "in method '" "estimateInertialParametersFromLinkBoundingBoxesAndTotalMass" "', argument " "1"" of type '" "double""'");
+  } 
+  arg1 = static_cast< double >(val1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_iDynTree__Model,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "estimateInertialParametersFromLinkBoundingBoxesAndTotalMass" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateInertialParametersFromLinkBoundingBoxesAndTotalMass" "', argument " "2"" of type '" "iDynTree::Model &""'"); 
+  }
+  arg2 = reinterpret_cast< iDynTree::Model * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "estimateInertialParametersFromLinkBoundingBoxesAndTotalMass" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "estimateInertialParametersFromLinkBoundingBoxesAndTotalMass" "', argument " "3"" of type '" "iDynTree::VectorDynSize &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  result = (bool)iDynTree::estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(arg1,*arg2,*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_DynamicsRegressorParameter_category_set(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   iDynTree::Regressors::DynamicsRegressorParameter *arg1 = (iDynTree::Regressors::DynamicsRegressorParameter *) 0 ;
   iDynTree::Regressors::DynamicsRegressorParameterCategory arg2 ;
@@ -94142,6 +95833,8 @@ static swig_type_info _swigt__p_iDynTree__MatrixFixSizeT_6_10_t = {"_p_iDynTree_
 static swig_type_info _swigt__p_iDynTree__MatrixFixSizeT_6_1_t = {"_p_iDynTree__MatrixFixSizeT_6_1_t", "iDynTree::MatrixFixSize< 6,1 > *|iDynTree::Matrix6x1 *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_iDynTree__MatrixFixSizeT_6_6_t = {"_p_iDynTree__MatrixFixSizeT_6_6_t", "iDynTree::MatrixFixSize< 6,6 > *|iDynTree::Matrix6x6 *", 0, 0, (void*)"iDynTree.Matrix6x6", 0};
 static swig_type_info _swigt__p_iDynTree__Model = {"_p_iDynTree__Model", "iDynTree::Model *", 0, 0, (void*)"iDynTree.Model", 0};
+static swig_type_info _swigt__p_iDynTree__ModelExporter = {"_p_iDynTree__ModelExporter", "iDynTree::ModelExporter *", 0, 0, (void*)"iDynTree.ModelExporter", 0};
+static swig_type_info _swigt__p_iDynTree__ModelExporterOptions = {"_p_iDynTree__ModelExporterOptions", "iDynTree::ModelExporterOptions *", 0, 0, (void*)"iDynTree.ModelExporterOptions", 0};
 static swig_type_info _swigt__p_iDynTree__ModelLoader = {"_p_iDynTree__ModelLoader", "iDynTree::ModelLoader *", 0, 0, (void*)"iDynTree.ModelLoader", 0};
 static swig_type_info _swigt__p_iDynTree__ModelParserOptions = {"_p_iDynTree__ModelParserOptions", "iDynTree::ModelParserOptions *", 0, 0, (void*)"iDynTree.ModelParserOptions", 0};
 static swig_type_info _swigt__p_iDynTree__ModelSolidShapes = {"_p_iDynTree__ModelSolidShapes", "iDynTree::ModelSolidShapes *", 0, 0, (void*)"iDynTree.ModelSolidShapes", 0};
@@ -94224,6 +95917,7 @@ static swig_type_info _swigt__p_std__allocatorT_iDynTree__BerdySensor_t = {"_p_s
 static swig_type_info _swigt__p_std__allocatorT_std__string_t = {"_p_std__allocatorT_std__string_t", "std::vector< std::string >::allocator_type *|std::allocator< std::string > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t = {"_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t", "iDynTree::Span< double,-1 >::reverse_iterator *|std::reverse_iterator< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,false > > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t = {"_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t", "std::reverse_iterator< iDynTree::details::span_iterator< iDynTree::Span< double,-1 >,true > > *|iDynTree::Span< double,-1 >::const_reverse_iterator *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_std__string = {"_p_std__string", "std::string *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t = {"_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t", "std::vector< iDynTree::BerdyDynamicVariable,std::allocator< iDynTree::BerdyDynamicVariable > > *|std::vector< iDynTree::BerdyDynamicVariable > *", 0, 0, (void*)"iDynTree.BerdyDynamicVariables", 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t = {"_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t", "std::vector< iDynTree::BerdySensor > *|std::vector< iDynTree::BerdySensor,std::allocator< iDynTree::BerdySensor > > *", 0, 0, (void*)"iDynTree.BerdySensors", 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t = {"_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t", "std::vector< iDynTree::MatrixDynSize,std::allocator< iDynTree::MatrixDynSize > > *", 0, 0, (void*)0, 0};
@@ -94231,6 +95925,7 @@ static swig_type_info _swigt__p_std__vectorT_iDynTree__Position_std__allocatorT_
 static swig_type_info _swigt__p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t = {"_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t", "std::vector< iDynTree::Regressors::DynamicsRegressorParameter,std::allocator< iDynTree::Regressors::DynamicsRegressorParameter > > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t = {"_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t", "std::vector< iDynTree::VectorDynSize,std::allocator< iDynTree::VectorDynSize > > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t = {"_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t", "std::vector< iDynTree::Vector6,std::allocator< iDynTree::Vector6 > > *|std::vector< iDynTree::VectorFixSize< 6 >,std::allocator< iDynTree::VectorFixSize< 6 > > > *", 0, 0, (void*)0, 0};
+static swig_type_info _swigt__p_std__vectorT_int_std__allocatorT_int_t_t = {"_p_std__vectorT_int_std__allocatorT_int_t_t", "std::vector< int,std::allocator< int > > *|std::vector< iDynTree::FrameIndex,std::allocator< iDynTree::FrameIndex > > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_std__vectorT_std__string_std__allocatorT_std__string_t_t = {"_p_std__vectorT_std__string_std__allocatorT_std__string_t_t", "std::vector< std::string,std::allocator< std::string > > *|std::vector< std::string > *", 0, 0, (void*)"iDynTree.StringVector", 0};
 static swig_type_info _swigt__p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t = {"_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t", "std::vector< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > >,std::allocator< std::vector< iDynTree::SolidShape *,std::allocator< iDynTree::SolidShape * > > > > *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_swig__MatlabSwigIterator = {"_p_swig__MatlabSwigIterator", "swig::MatlabSwigIterator *", 0, 0, (void*)"iDynTree.MatlabSwigIterator", 0};
@@ -94356,6 +96051,8 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__MatrixFixSizeT_6_1_t,
   &_swigt__p_iDynTree__MatrixFixSizeT_6_6_t,
   &_swigt__p_iDynTree__Model,
+  &_swigt__p_iDynTree__ModelExporter,
+  &_swigt__p_iDynTree__ModelExporterOptions,
   &_swigt__p_iDynTree__ModelLoader,
   &_swigt__p_iDynTree__ModelParserOptions,
   &_swigt__p_iDynTree__ModelSolidShapes,
@@ -94438,6 +96135,7 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_std__allocatorT_std__string_t,
   &_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t,
   &_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t,
+  &_swigt__p_std__string,
   &_swigt__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t,
   &_swigt__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t,
   &_swigt__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t,
@@ -94445,6 +96143,7 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t,
   &_swigt__p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t,
   &_swigt__p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t,
+  &_swigt__p_std__vectorT_int_std__allocatorT_int_t_t,
   &_swigt__p_std__vectorT_std__string_std__allocatorT_std__string_t_t,
   &_swigt__p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t,
   &_swigt__p_swig__MatlabSwigIterator,
@@ -94570,6 +96269,8 @@ static swig_cast_info _swigc__p_iDynTree__MatrixFixSizeT_6_10_t[] = {  {&_swigt_
 static swig_cast_info _swigc__p_iDynTree__MatrixFixSizeT_6_1_t[] = {  {&_swigt__p_iDynTree__MatrixFixSizeT_6_1_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__MatrixFixSizeT_6_6_t[] = {  {&_swigt__p_iDynTree__MatrixFixSizeT_6_6_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__Model[] = {  {&_swigt__p_iDynTree__Model, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__ModelExporter[] = {  {&_swigt__p_iDynTree__ModelExporter, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__ModelExporterOptions[] = {  {&_swigt__p_iDynTree__ModelExporterOptions, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ModelLoader[] = {  {&_swigt__p_iDynTree__ModelLoader, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ModelParserOptions[] = {  {&_swigt__p_iDynTree__ModelParserOptions, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ModelSolidShapes[] = {  {&_swigt__p_iDynTree__ModelSolidShapes, 0, 0, 0},{0, 0, 0, 0}};
@@ -94652,6 +96353,7 @@ static swig_cast_info _swigc__p_std__allocatorT_iDynTree__BerdySensor_t[] = {  {
 static swig_cast_info _swigc__p_std__allocatorT_std__string_t[] = {  {&_swigt__p_std__allocatorT_std__string_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t[] = {  {&_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t[] = {  {&_swigt__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_std__string[] = {  {&_swigt__p_std__string, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t, 0, 0, 0},{0, 0, 0, 0}};
@@ -94659,6 +96361,7 @@ static swig_cast_info _swigc__p_std__vectorT_iDynTree__Position_std__allocatorT_
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t[] = {  {&_swigt__p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_std__vectorT_int_std__allocatorT_int_t_t[] = {  {&_swigt__p_std__vectorT_int_std__allocatorT_int_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_std__string_std__allocatorT_std__string_t_t[] = {  {&_swigt__p_std__vectorT_std__string_std__allocatorT_std__string_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t[] = {  {&_swigt__p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_swig__MatlabSwigIterator[] = {  {&_swigt__p_swig__MatlabSwigIterator, 0, 0, 0},{0, 0, 0, 0}};
@@ -94784,6 +96487,8 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__MatrixFixSizeT_6_1_t,
   _swigc__p_iDynTree__MatrixFixSizeT_6_6_t,
   _swigc__p_iDynTree__Model,
+  _swigc__p_iDynTree__ModelExporter,
+  _swigc__p_iDynTree__ModelExporterOptions,
   _swigc__p_iDynTree__ModelLoader,
   _swigc__p_iDynTree__ModelParserOptions,
   _swigc__p_iDynTree__ModelSolidShapes,
@@ -94866,6 +96571,7 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_std__allocatorT_std__string_t,
   _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t,
   _swigc__p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t,
+  _swigc__p_std__string,
   _swigc__p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t,
   _swigc__p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t,
   _swigc__p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t,
@@ -94873,6 +96579,7 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t,
   _swigc__p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t,
   _swigc__p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t,
+  _swigc__p_std__vectorT_int_std__allocatorT_int_t_t,
   _swigc__p_std__vectorT_std__string_std__allocatorT_std__string_t_t,
   _swigc__p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t,
   _swigc__p_swig__MatlabSwigIterator,
@@ -96049,1219 +97756,1245 @@ SWIGINTERN const char* SwigFunctionName(int fcn_id) {
   case 765: return "Rotation_RotAxisDerivative";
   case 766: return "Rotation_RPY";
   case 767: return "Rotation_RPYRightTrivializedDerivative";
-  case 768: return "Rotation_RPYRightTrivializedDerivativeInverse";
-  case 769: return "Rotation_QuaternionRightTrivializedDerivative";
-  case 770: return "Rotation_QuaternionRightTrivializedDerivativeInverse";
-  case 771: return "Rotation_Identity";
-  case 772: return "Rotation_RotationFromQuaternion";
-  case 773: return "Rotation_toString";
-  case 774: return "Rotation_display";
-  case 775: return "delete_Rotation";
-  case 776: return "new_TransformSemantics";
-  case 777: return "TransformSemantics_getRotationSemantics";
-  case 778: return "TransformSemantics_getPositionSemantics";
-  case 779: return "TransformSemantics_setRotationSemantics";
-  case 780: return "TransformSemantics_setPositionSemantics";
-  case 781: return "TransformSemantics_toString";
-  case 782: return "TransformSemantics_display";
-  case 783: return "delete_TransformSemantics";
-  case 784: return "new_Transform";
-  case 785: return "Transform_fromHomogeneousTransform";
-  case 786: return "Transform_getSemantics";
-  case 787: return "Transform_getRotation";
-  case 788: return "Transform_getPosition";
-  case 789: return "Transform_setRotation";
-  case 790: return "Transform_setPosition";
-  case 791: return "Transform_compose";
-  case 792: return "Transform_inverse2";
-  case 793: return "Transform_inverse";
-  case 794: return "Transform_mtimes";
-  case 795: return "Transform_Identity";
-  case 796: return "Transform_asHomogeneousTransform";
-  case 797: return "Transform_asAdjointTransform";
-  case 798: return "Transform_asAdjointTransformWrench";
-  case 799: return "Transform_log";
-  case 800: return "Transform_toString";
-  case 801: return "Transform_display";
-  case 802: return "delete_Transform";
-  case 803: return "new_TransformDerivative";
-  case 804: return "delete_TransformDerivative";
-  case 805: return "TransformDerivative_getRotationDerivative";
-  case 806: return "TransformDerivative_getPositionDerivative";
-  case 807: return "TransformDerivative_setRotationDerivative";
-  case 808: return "TransformDerivative_setPositionDerivative";
-  case 809: return "TransformDerivative_Zero";
-  case 810: return "TransformDerivative_asHomogeneousTransformDerivative";
-  case 811: return "TransformDerivative_asAdjointTransformDerivative";
-  case 812: return "TransformDerivative_asAdjointTransformWrenchDerivative";
-  case 813: return "TransformDerivative_mtimes";
-  case 814: return "TransformDerivative_derivativeOfInverse";
-  case 815: return "TransformDerivative_transform";
-  case 816: return "dynamic_extent_get";
-  case 817: return "DynamicSpan_extent_get";
-  case 818: return "new_DynamicSpan";
-  case 819: return "delete_DynamicSpan";
-  case 820: return "DynamicSpan_first";
-  case 821: return "DynamicSpan_last";
-  case 822: return "DynamicSpan_subspan";
-  case 823: return "DynamicSpan_size";
-  case 824: return "DynamicSpan_size_bytes";
-  case 825: return "DynamicSpan_empty";
-  case 826: return "DynamicSpan_brace";
-  case 827: return "DynamicSpan_getVal";
-  case 828: return "DynamicSpan_setVal";
-  case 829: return "DynamicSpan_at";
-  case 830: return "DynamicSpan_paren";
-  case 831: return "DynamicSpan_begin";
-  case 832: return "DynamicSpan_end";
-  case 833: return "DynamicSpan_cbegin";
-  case 834: return "DynamicSpan_cend";
-  case 835: return "DynamicSpan_rbegin";
-  case 836: return "DynamicSpan_rend";
-  case 837: return "DynamicSpan_crbegin";
-  case 838: return "DynamicSpan_crend";
-  case 839: return "LINK_INVALID_INDEX_get";
-  case 840: return "LINK_INVALID_INDEX_set";
-  case 841: return "LINK_INVALID_NAME_get";
-  case 842: return "LINK_INVALID_NAME_set";
-  case 843: return "JOINT_INVALID_INDEX_get";
-  case 844: return "JOINT_INVALID_INDEX_set";
-  case 845: return "JOINT_INVALID_NAME_get";
-  case 846: return "JOINT_INVALID_NAME_set";
-  case 847: return "DOF_INVALID_INDEX_get";
-  case 848: return "DOF_INVALID_INDEX_set";
-  case 849: return "DOF_INVALID_NAME_get";
-  case 850: return "DOF_INVALID_NAME_set";
-  case 851: return "FRAME_INVALID_INDEX_get";
-  case 852: return "FRAME_INVALID_INDEX_set";
-  case 853: return "FRAME_INVALID_NAME_get";
-  case 854: return "FRAME_INVALID_NAME_set";
-  case 855: return "TRAVERSAL_INVALID_INDEX_get";
-  case 856: return "TRAVERSAL_INVALID_INDEX_set";
-  case 857: return "new_LinkPositions";
-  case 858: return "LinkPositions_resize";
-  case 859: return "LinkPositions_isConsistent";
-  case 860: return "LinkPositions_getNrOfLinks";
-  case 861: return "LinkPositions_paren";
-  case 862: return "LinkPositions_toString";
-  case 863: return "delete_LinkPositions";
-  case 864: return "new_LinkWrenches";
-  case 865: return "LinkWrenches_resize";
-  case 866: return "LinkWrenches_isConsistent";
-  case 867: return "LinkWrenches_getNrOfLinks";
-  case 868: return "LinkWrenches_paren";
-  case 869: return "LinkWrenches_toString";
-  case 870: return "LinkWrenches_zero";
-  case 871: return "delete_LinkWrenches";
-  case 872: return "new_LinkInertias";
-  case 873: return "LinkInertias_resize";
-  case 874: return "LinkInertias_isConsistent";
-  case 875: return "LinkInertias_paren";
-  case 876: return "delete_LinkInertias";
-  case 877: return "new_LinkArticulatedBodyInertias";
-  case 878: return "LinkArticulatedBodyInertias_resize";
-  case 879: return "LinkArticulatedBodyInertias_isConsistent";
-  case 880: return "LinkArticulatedBodyInertias_paren";
-  case 881: return "delete_LinkArticulatedBodyInertias";
-  case 882: return "new_LinkVelArray";
-  case 883: return "LinkVelArray_resize";
-  case 884: return "LinkVelArray_isConsistent";
-  case 885: return "LinkVelArray_getNrOfLinks";
-  case 886: return "LinkVelArray_paren";
-  case 887: return "LinkVelArray_toString";
-  case 888: return "delete_LinkVelArray";
-  case 889: return "new_LinkAccArray";
-  case 890: return "LinkAccArray_resize";
-  case 891: return "LinkAccArray_isConsistent";
-  case 892: return "LinkAccArray_paren";
-  case 893: return "LinkAccArray_getNrOfLinks";
-  case 894: return "LinkAccArray_toString";
-  case 895: return "delete_LinkAccArray";
-  case 896: return "new_Link";
-  case 897: return "Link_inertia";
-  case 898: return "Link_setInertia";
-  case 899: return "Link_getInertia";
-  case 900: return "Link_setIndex";
-  case 901: return "Link_getIndex";
-  case 902: return "delete_Link";
-  case 903: return "delete_IJoint";
-  case 904: return "IJoint_clone";
-  case 905: return "IJoint_getNrOfPosCoords";
-  case 906: return "IJoint_getNrOfDOFs";
-  case 907: return "IJoint_setAttachedLinks";
-  case 908: return "IJoint_setRestTransform";
-  case 909: return "IJoint_getFirstAttachedLink";
-  case 910: return "IJoint_getSecondAttachedLink";
-  case 911: return "IJoint_getRestTransform";
-  case 912: return "IJoint_getTransform";
-  case 913: return "IJoint_getTransformDerivative";
-  case 914: return "IJoint_getMotionSubspaceVector";
-  case 915: return "IJoint_computeChildPosVelAcc";
-  case 916: return "IJoint_computeChildVelAcc";
-  case 917: return "IJoint_computeChildVel";
-  case 918: return "IJoint_computeChildAcc";
-  case 919: return "IJoint_computeChildBiasAcc";
-  case 920: return "IJoint_computeJointTorque";
-  case 921: return "IJoint_setIndex";
-  case 922: return "IJoint_getIndex";
-  case 923: return "IJoint_setPosCoordsOffset";
-  case 924: return "IJoint_getPosCoordsOffset";
-  case 925: return "IJoint_setDOFsOffset";
-  case 926: return "IJoint_getDOFsOffset";
-  case 927: return "IJoint_hasPosLimits";
-  case 928: return "IJoint_enablePosLimits";
-  case 929: return "IJoint_getPosLimits";
-  case 930: return "IJoint_getMinPosLimit";
-  case 931: return "IJoint_getMaxPosLimit";
-  case 932: return "IJoint_setPosLimits";
-  case 933: return "IJoint_isRevoluteJoint";
-  case 934: return "IJoint_isFixedJoint";
-  case 935: return "IJoint_asRevoluteJoint";
-  case 936: return "IJoint_asFixedJoint";
-  case 937: return "new_FixedJoint";
-  case 938: return "delete_FixedJoint";
-  case 939: return "FixedJoint_clone";
-  case 940: return "FixedJoint_getNrOfPosCoords";
-  case 941: return "FixedJoint_getNrOfDOFs";
-  case 942: return "FixedJoint_setAttachedLinks";
-  case 943: return "FixedJoint_setRestTransform";
-  case 944: return "FixedJoint_getFirstAttachedLink";
-  case 945: return "FixedJoint_getSecondAttachedLink";
-  case 946: return "FixedJoint_getRestTransform";
-  case 947: return "FixedJoint_getTransform";
-  case 948: return "FixedJoint_getTransformDerivative";
-  case 949: return "FixedJoint_getMotionSubspaceVector";
-  case 950: return "FixedJoint_computeChildPosVelAcc";
-  case 951: return "FixedJoint_computeChildVelAcc";
-  case 952: return "FixedJoint_computeChildVel";
-  case 953: return "FixedJoint_computeChildAcc";
-  case 954: return "FixedJoint_computeChildBiasAcc";
-  case 955: return "FixedJoint_computeJointTorque";
-  case 956: return "FixedJoint_setIndex";
-  case 957: return "FixedJoint_getIndex";
-  case 958: return "FixedJoint_setPosCoordsOffset";
-  case 959: return "FixedJoint_getPosCoordsOffset";
-  case 960: return "FixedJoint_setDOFsOffset";
-  case 961: return "FixedJoint_getDOFsOffset";
-  case 962: return "FixedJoint_hasPosLimits";
-  case 963: return "FixedJoint_enablePosLimits";
-  case 964: return "FixedJoint_getPosLimits";
-  case 965: return "FixedJoint_getMinPosLimit";
-  case 966: return "FixedJoint_getMaxPosLimit";
-  case 967: return "FixedJoint_setPosLimits";
-  case 968: return "delete_MovableJointImpl1";
-  case 969: return "MovableJointImpl1_getNrOfPosCoords";
-  case 970: return "MovableJointImpl1_getNrOfDOFs";
-  case 971: return "MovableJointImpl1_setIndex";
-  case 972: return "MovableJointImpl1_getIndex";
-  case 973: return "MovableJointImpl1_setPosCoordsOffset";
-  case 974: return "MovableJointImpl1_getPosCoordsOffset";
-  case 975: return "MovableJointImpl1_setDOFsOffset";
-  case 976: return "MovableJointImpl1_getDOFsOffset";
-  case 977: return "delete_MovableJointImpl2";
-  case 978: return "MovableJointImpl2_getNrOfPosCoords";
-  case 979: return "MovableJointImpl2_getNrOfDOFs";
-  case 980: return "MovableJointImpl2_setIndex";
-  case 981: return "MovableJointImpl2_getIndex";
-  case 982: return "MovableJointImpl2_setPosCoordsOffset";
-  case 983: return "MovableJointImpl2_getPosCoordsOffset";
-  case 984: return "MovableJointImpl2_setDOFsOffset";
-  case 985: return "MovableJointImpl2_getDOFsOffset";
-  case 986: return "delete_MovableJointImpl3";
-  case 987: return "MovableJointImpl3_getNrOfPosCoords";
-  case 988: return "MovableJointImpl3_getNrOfDOFs";
-  case 989: return "MovableJointImpl3_setIndex";
-  case 990: return "MovableJointImpl3_getIndex";
-  case 991: return "MovableJointImpl3_setPosCoordsOffset";
-  case 992: return "MovableJointImpl3_getPosCoordsOffset";
-  case 993: return "MovableJointImpl3_setDOFsOffset";
-  case 994: return "MovableJointImpl3_getDOFsOffset";
-  case 995: return "delete_MovableJointImpl4";
-  case 996: return "MovableJointImpl4_getNrOfPosCoords";
-  case 997: return "MovableJointImpl4_getNrOfDOFs";
-  case 998: return "MovableJointImpl4_setIndex";
-  case 999: return "MovableJointImpl4_getIndex";
-  case 1000: return "MovableJointImpl4_setPosCoordsOffset";
-  case 1001: return "MovableJointImpl4_getPosCoordsOffset";
-  case 1002: return "MovableJointImpl4_setDOFsOffset";
-  case 1003: return "MovableJointImpl4_getDOFsOffset";
-  case 1004: return "delete_MovableJointImpl5";
-  case 1005: return "MovableJointImpl5_getNrOfPosCoords";
-  case 1006: return "MovableJointImpl5_getNrOfDOFs";
-  case 1007: return "MovableJointImpl5_setIndex";
-  case 1008: return "MovableJointImpl5_getIndex";
-  case 1009: return "MovableJointImpl5_setPosCoordsOffset";
-  case 1010: return "MovableJointImpl5_getPosCoordsOffset";
-  case 1011: return "MovableJointImpl5_setDOFsOffset";
-  case 1012: return "MovableJointImpl5_getDOFsOffset";
-  case 1013: return "delete_MovableJointImpl6";
-  case 1014: return "MovableJointImpl6_getNrOfPosCoords";
-  case 1015: return "MovableJointImpl6_getNrOfDOFs";
-  case 1016: return "MovableJointImpl6_setIndex";
-  case 1017: return "MovableJointImpl6_getIndex";
-  case 1018: return "MovableJointImpl6_setPosCoordsOffset";
-  case 1019: return "MovableJointImpl6_getPosCoordsOffset";
-  case 1020: return "MovableJointImpl6_setDOFsOffset";
-  case 1021: return "MovableJointImpl6_getDOFsOffset";
-  case 1022: return "new_RevoluteJoint";
-  case 1023: return "delete_RevoluteJoint";
-  case 1024: return "RevoluteJoint_clone";
-  case 1025: return "RevoluteJoint_setAttachedLinks";
-  case 1026: return "RevoluteJoint_setRestTransform";
-  case 1027: return "RevoluteJoint_setAxis";
-  case 1028: return "RevoluteJoint_getFirstAttachedLink";
-  case 1029: return "RevoluteJoint_getSecondAttachedLink";
-  case 1030: return "RevoluteJoint_getAxis";
-  case 1031: return "RevoluteJoint_getRestTransform";
-  case 1032: return "RevoluteJoint_getTransform";
-  case 1033: return "RevoluteJoint_getTransformDerivative";
-  case 1034: return "RevoluteJoint_getMotionSubspaceVector";
-  case 1035: return "RevoluteJoint_computeChildPosVelAcc";
-  case 1036: return "RevoluteJoint_computeChildVel";
-  case 1037: return "RevoluteJoint_computeChildVelAcc";
-  case 1038: return "RevoluteJoint_computeChildAcc";
-  case 1039: return "RevoluteJoint_computeChildBiasAcc";
-  case 1040: return "RevoluteJoint_computeJointTorque";
-  case 1041: return "RevoluteJoint_hasPosLimits";
-  case 1042: return "RevoluteJoint_enablePosLimits";
-  case 1043: return "RevoluteJoint_getPosLimits";
-  case 1044: return "RevoluteJoint_getMinPosLimit";
-  case 1045: return "RevoluteJoint_getMaxPosLimit";
-  case 1046: return "RevoluteJoint_setPosLimits";
-  case 1047: return "new_PrismaticJoint";
-  case 1048: return "delete_PrismaticJoint";
-  case 1049: return "PrismaticJoint_clone";
-  case 1050: return "PrismaticJoint_setAttachedLinks";
-  case 1051: return "PrismaticJoint_setRestTransform";
-  case 1052: return "PrismaticJoint_setAxis";
-  case 1053: return "PrismaticJoint_getFirstAttachedLink";
-  case 1054: return "PrismaticJoint_getSecondAttachedLink";
-  case 1055: return "PrismaticJoint_getAxis";
-  case 1056: return "PrismaticJoint_getRestTransform";
-  case 1057: return "PrismaticJoint_getTransform";
-  case 1058: return "PrismaticJoint_getTransformDerivative";
-  case 1059: return "PrismaticJoint_getMotionSubspaceVector";
-  case 1060: return "PrismaticJoint_computeChildPosVelAcc";
-  case 1061: return "PrismaticJoint_computeChildVel";
-  case 1062: return "PrismaticJoint_computeChildVelAcc";
-  case 1063: return "PrismaticJoint_computeChildAcc";
-  case 1064: return "PrismaticJoint_computeChildBiasAcc";
-  case 1065: return "PrismaticJoint_computeJointTorque";
-  case 1066: return "PrismaticJoint_hasPosLimits";
-  case 1067: return "PrismaticJoint_enablePosLimits";
-  case 1068: return "PrismaticJoint_getPosLimits";
-  case 1069: return "PrismaticJoint_getMinPosLimit";
-  case 1070: return "PrismaticJoint_getMaxPosLimit";
-  case 1071: return "PrismaticJoint_setPosLimits";
-  case 1072: return "new_Traversal";
-  case 1073: return "delete_Traversal";
-  case 1074: return "Traversal_getNrOfVisitedLinks";
-  case 1075: return "Traversal_getLink";
-  case 1076: return "Traversal_getBaseLink";
-  case 1077: return "Traversal_getParentLink";
-  case 1078: return "Traversal_getParentJoint";
-  case 1079: return "Traversal_getParentLinkFromLinkIndex";
-  case 1080: return "Traversal_getParentJointFromLinkIndex";
-  case 1081: return "Traversal_getTraversalIndexFromLinkIndex";
-  case 1082: return "Traversal_reset";
-  case 1083: return "Traversal_addTraversalBase";
-  case 1084: return "Traversal_addTraversalElement";
-  case 1085: return "Traversal_isParentOf";
-  case 1086: return "Traversal_getChildLinkIndexFromJointIndex";
-  case 1087: return "Traversal_getParentLinkIndexFromJointIndex";
-  case 1088: return "Traversal_toString";
-  case 1089: return "delete_SolidShape";
-  case 1090: return "SolidShape_clone";
-  case 1091: return "SolidShape_name_get";
-  case 1092: return "SolidShape_name_set";
-  case 1093: return "SolidShape_link_H_geometry_get";
-  case 1094: return "SolidShape_link_H_geometry_set";
-  case 1095: return "SolidShape_material_get";
-  case 1096: return "SolidShape_material_set";
-  case 1097: return "SolidShape_isSphere";
-  case 1098: return "SolidShape_isBox";
-  case 1099: return "SolidShape_isCylinder";
-  case 1100: return "SolidShape_isExternalMesh";
-  case 1101: return "SolidShape_asSphere";
-  case 1102: return "SolidShape_asBox";
-  case 1103: return "SolidShape_asCylinder";
-  case 1104: return "SolidShape_asExternalMesh";
-  case 1105: return "delete_Sphere";
-  case 1106: return "Sphere_clone";
-  case 1107: return "Sphere_radius_get";
-  case 1108: return "Sphere_radius_set";
-  case 1109: return "new_Sphere";
-  case 1110: return "delete_Box";
-  case 1111: return "Box_clone";
-  case 1112: return "Box_x_get";
-  case 1113: return "Box_x_set";
-  case 1114: return "Box_y_get";
-  case 1115: return "Box_y_set";
-  case 1116: return "Box_z_get";
-  case 1117: return "Box_z_set";
-  case 1118: return "new_Box";
-  case 1119: return "delete_Cylinder";
-  case 1120: return "Cylinder_clone";
-  case 1121: return "Cylinder_length_get";
-  case 1122: return "Cylinder_length_set";
-  case 1123: return "Cylinder_radius_get";
-  case 1124: return "Cylinder_radius_set";
-  case 1125: return "new_Cylinder";
-  case 1126: return "delete_ExternalMesh";
-  case 1127: return "ExternalMesh_clone";
-  case 1128: return "ExternalMesh_filename_get";
-  case 1129: return "ExternalMesh_filename_set";
-  case 1130: return "ExternalMesh_scale_get";
-  case 1131: return "ExternalMesh_scale_set";
-  case 1132: return "new_ExternalMesh";
-  case 1133: return "new_ModelSolidShapes";
-  case 1134: return "ModelSolidShapes_clear";
-  case 1135: return "delete_ModelSolidShapes";
-  case 1136: return "ModelSolidShapes_resize";
-  case 1137: return "ModelSolidShapes_isConsistent";
-  case 1138: return "ModelSolidShapes_linkSolidShapes_get";
-  case 1139: return "ModelSolidShapes_linkSolidShapes_set";
-  case 1140: return "Neighbor_neighborLink_get";
-  case 1141: return "Neighbor_neighborLink_set";
-  case 1142: return "Neighbor_neighborJoint_get";
-  case 1143: return "Neighbor_neighborJoint_set";
-  case 1144: return "new_Neighbor";
-  case 1145: return "delete_Neighbor";
-  case 1146: return "new_Model";
-  case 1147: return "Model_copy";
-  case 1148: return "delete_Model";
-  case 1149: return "Model_getNrOfLinks";
-  case 1150: return "Model_getLinkName";
-  case 1151: return "Model_getLinkIndex";
-  case 1152: return "Model_isValidLinkIndex";
-  case 1153: return "Model_getLink";
-  case 1154: return "Model_addLink";
-  case 1155: return "Model_getNrOfJoints";
-  case 1156: return "Model_getJointName";
-  case 1157: return "Model_getJointIndex";
-  case 1158: return "Model_getJoint";
-  case 1159: return "Model_isValidJointIndex";
-  case 1160: return "Model_isLinkNameUsed";
-  case 1161: return "Model_isJointNameUsed";
-  case 1162: return "Model_isFrameNameUsed";
-  case 1163: return "Model_addJoint";
-  case 1164: return "Model_addJointAndLink";
-  case 1165: return "Model_insertLinkToExistingJointAndAddJointForDisplacedLink";
-  case 1166: return "Model_getNrOfPosCoords";
-  case 1167: return "Model_getNrOfDOFs";
-  case 1168: return "Model_getNrOfFrames";
-  case 1169: return "Model_addAdditionalFrameToLink";
-  case 1170: return "Model_getFrameName";
-  case 1171: return "Model_getFrameIndex";
-  case 1172: return "Model_isValidFrameIndex";
-  case 1173: return "Model_getFrameTransform";
-  case 1174: return "Model_getFrameLink";
-  case 1175: return "Model_getNrOfNeighbors";
-  case 1176: return "Model_getNeighbor";
-  case 1177: return "Model_setDefaultBaseLink";
-  case 1178: return "Model_getDefaultBaseLink";
-  case 1179: return "Model_computeFullTreeTraversal";
-  case 1180: return "Model_getInertialParameters";
-  case 1181: return "Model_updateInertialParameters";
-  case 1182: return "Model_visualSolidShapes";
-  case 1183: return "Model_collisionSolidShapes";
-  case 1184: return "Model_toString";
-  case 1185: return "new_JointPosDoubleArray";
-  case 1186: return "JointPosDoubleArray_resize";
-  case 1187: return "JointPosDoubleArray_isConsistent";
-  case 1188: return "delete_JointPosDoubleArray";
-  case 1189: return "new_JointDOFsDoubleArray";
-  case 1190: return "JointDOFsDoubleArray_resize";
-  case 1191: return "JointDOFsDoubleArray_isConsistent";
-  case 1192: return "delete_JointDOFsDoubleArray";
-  case 1193: return "new_DOFSpatialForceArray";
-  case 1194: return "DOFSpatialForceArray_resize";
-  case 1195: return "DOFSpatialForceArray_isConsistent";
-  case 1196: return "DOFSpatialForceArray_paren";
-  case 1197: return "delete_DOFSpatialForceArray";
-  case 1198: return "new_DOFSpatialMotionArray";
-  case 1199: return "DOFSpatialMotionArray_resize";
-  case 1200: return "DOFSpatialMotionArray_isConsistent";
-  case 1201: return "DOFSpatialMotionArray_paren";
-  case 1202: return "delete_DOFSpatialMotionArray";
-  case 1203: return "new_FrameFreeFloatingJacobian";
-  case 1204: return "FrameFreeFloatingJacobian_resize";
-  case 1205: return "FrameFreeFloatingJacobian_isConsistent";
-  case 1206: return "delete_FrameFreeFloatingJacobian";
-  case 1207: return "new_MomentumFreeFloatingJacobian";
-  case 1208: return "MomentumFreeFloatingJacobian_resize";
-  case 1209: return "MomentumFreeFloatingJacobian_isConsistent";
-  case 1210: return "delete_MomentumFreeFloatingJacobian";
-  case 1211: return "new_FreeFloatingMassMatrix";
-  case 1212: return "FreeFloatingMassMatrix_resize";
-  case 1213: return "delete_FreeFloatingMassMatrix";
-  case 1214: return "new_FreeFloatingPos";
-  case 1215: return "FreeFloatingPos_resize";
-  case 1216: return "FreeFloatingPos_worldBasePos";
-  case 1217: return "FreeFloatingPos_jointPos";
-  case 1218: return "FreeFloatingPos_getNrOfPosCoords";
-  case 1219: return "delete_FreeFloatingPos";
-  case 1220: return "new_FreeFloatingGeneralizedTorques";
-  case 1221: return "FreeFloatingGeneralizedTorques_resize";
-  case 1222: return "FreeFloatingGeneralizedTorques_baseWrench";
-  case 1223: return "FreeFloatingGeneralizedTorques_jointTorques";
-  case 1224: return "FreeFloatingGeneralizedTorques_getNrOfDOFs";
-  case 1225: return "delete_FreeFloatingGeneralizedTorques";
-  case 1226: return "new_FreeFloatingVel";
-  case 1227: return "FreeFloatingVel_resize";
-  case 1228: return "FreeFloatingVel_baseVel";
-  case 1229: return "FreeFloatingVel_jointVel";
-  case 1230: return "FreeFloatingVel_getNrOfDOFs";
-  case 1231: return "delete_FreeFloatingVel";
-  case 1232: return "new_FreeFloatingAcc";
-  case 1233: return "FreeFloatingAcc_resize";
-  case 1234: return "FreeFloatingAcc_baseAcc";
-  case 1235: return "FreeFloatingAcc_jointAcc";
-  case 1236: return "FreeFloatingAcc_getNrOfDOFs";
-  case 1237: return "delete_FreeFloatingAcc";
-  case 1238: return "ContactWrench_contactId";
-  case 1239: return "ContactWrench_contactPoint";
-  case 1240: return "ContactWrench_contactWrench";
-  case 1241: return "new_ContactWrench";
-  case 1242: return "delete_ContactWrench";
-  case 1243: return "new_LinkContactWrenches";
-  case 1244: return "LinkContactWrenches_resize";
-  case 1245: return "LinkContactWrenches_getNrOfContactsForLink";
-  case 1246: return "LinkContactWrenches_setNrOfContactsForLink";
-  case 1247: return "LinkContactWrenches_getNrOfLinks";
-  case 1248: return "LinkContactWrenches_contactWrench";
-  case 1249: return "LinkContactWrenches_computeNetWrenches";
-  case 1250: return "LinkContactWrenches_toString";
-  case 1251: return "delete_LinkContactWrenches";
-  case 1252: return "_wrap_ForwardPositionKinematics";
-  case 1253: return "_wrap_ForwardVelAccKinematics";
-  case 1254: return "_wrap_ForwardPosVelAccKinematics";
-  case 1255: return "_wrap_ForwardPosVelKinematics";
-  case 1256: return "_wrap_ForwardAccKinematics";
-  case 1257: return "_wrap_ForwardBiasAccKinematics";
-  case 1258: return "_wrap_ComputeLinearAndAngularMomentum";
-  case 1259: return "_wrap_ComputeLinearAndAngularMomentumDerivativeBias";
-  case 1260: return "_wrap_RNEADynamicPhase";
-  case 1261: return "_wrap_CompositeRigidBodyAlgorithm";
-  case 1262: return "new_ArticulatedBodyAlgorithmInternalBuffers";
-  case 1263: return "ArticulatedBodyAlgorithmInternalBuffers_resize";
-  case 1264: return "ArticulatedBodyAlgorithmInternalBuffers_isConsistent";
-  case 1265: return "ArticulatedBodyAlgorithmInternalBuffers_S_get";
-  case 1266: return "ArticulatedBodyAlgorithmInternalBuffers_S_set";
-  case 1267: return "ArticulatedBodyAlgorithmInternalBuffers_U_get";
-  case 1268: return "ArticulatedBodyAlgorithmInternalBuffers_U_set";
-  case 1269: return "ArticulatedBodyAlgorithmInternalBuffers_D_get";
-  case 1270: return "ArticulatedBodyAlgorithmInternalBuffers_D_set";
-  case 1271: return "ArticulatedBodyAlgorithmInternalBuffers_u_get";
-  case 1272: return "ArticulatedBodyAlgorithmInternalBuffers_u_set";
-  case 1273: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_get";
-  case 1274: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set";
-  case 1275: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get";
-  case 1276: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set";
-  case 1277: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get";
-  case 1278: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set";
-  case 1279: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get";
-  case 1280: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set";
-  case 1281: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get";
-  case 1282: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set";
-  case 1283: return "delete_ArticulatedBodyAlgorithmInternalBuffers";
-  case 1284: return "_wrap_ArticulatedBodyAlgorithm";
-  case 1285: return "_wrap_InverseDynamicsInertialParametersRegressor";
-  case 1286: return "NR_OF_SENSOR_TYPES_get";
-  case 1287: return "_wrap_isLinkSensor";
-  case 1288: return "_wrap_isJointSensor";
-  case 1289: return "_wrap_getSensorTypeSize";
-  case 1290: return "delete_Sensor";
-  case 1291: return "Sensor_getName";
-  case 1292: return "Sensor_getSensorType";
-  case 1293: return "Sensor_isValid";
-  case 1294: return "Sensor_setName";
-  case 1295: return "Sensor_clone";
-  case 1296: return "Sensor_isConsistent";
-  case 1297: return "Sensor_updateIndices";
-  case 1298: return "Sensor_updateIndeces";
-  case 1299: return "delete_JointSensor";
-  case 1300: return "JointSensor_getParentJoint";
-  case 1301: return "JointSensor_getParentJointIndex";
-  case 1302: return "JointSensor_setParentJoint";
-  case 1303: return "JointSensor_setParentJointIndex";
-  case 1304: return "JointSensor_isConsistent";
-  case 1305: return "delete_LinkSensor";
-  case 1306: return "LinkSensor_getParentLink";
-  case 1307: return "LinkSensor_getParentLinkIndex";
-  case 1308: return "LinkSensor_getLinkSensorTransform";
-  case 1309: return "LinkSensor_setParentLink";
-  case 1310: return "LinkSensor_setParentLinkIndex";
-  case 1311: return "LinkSensor_setLinkSensorTransform";
-  case 1312: return "LinkSensor_isConsistent";
-  case 1313: return "new_SensorsList";
-  case 1314: return "delete_SensorsList";
-  case 1315: return "SensorsList_addSensor";
-  case 1316: return "SensorsList_setSerialization";
-  case 1317: return "SensorsList_getSerialization";
-  case 1318: return "SensorsList_getNrOfSensors";
-  case 1319: return "SensorsList_getSensorIndex";
-  case 1320: return "SensorsList_getSizeOfAllSensorsMeasurements";
-  case 1321: return "SensorsList_getSensor";
-  case 1322: return "SensorsList_isConsistent";
-  case 1323: return "SensorsList_removeSensor";
-  case 1324: return "SensorsList_removeAllSensorsOfType";
-  case 1325: return "SensorsList_getSixAxisForceTorqueSensor";
-  case 1326: return "SensorsList_getAccelerometerSensor";
-  case 1327: return "SensorsList_getGyroscopeSensor";
-  case 1328: return "SensorsList_getThreeAxisAngularAccelerometerSensor";
-  case 1329: return "SensorsList_getThreeAxisForceTorqueContactSensor";
-  case 1330: return "new_SensorsMeasurements";
-  case 1331: return "delete_SensorsMeasurements";
-  case 1332: return "SensorsMeasurements_setNrOfSensors";
-  case 1333: return "SensorsMeasurements_getNrOfSensors";
-  case 1334: return "SensorsMeasurements_resize";
-  case 1335: return "SensorsMeasurements_toVector";
-  case 1336: return "SensorsMeasurements_setMeasurement";
-  case 1337: return "SensorsMeasurements_getMeasurement";
-  case 1338: return "SensorsMeasurements_getSizeOfAllSensorsMeasurements";
-  case 1339: return "new_SixAxisForceTorqueSensor";
-  case 1340: return "delete_SixAxisForceTorqueSensor";
-  case 1341: return "SixAxisForceTorqueSensor_setName";
-  case 1342: return "SixAxisForceTorqueSensor_setFirstLinkSensorTransform";
-  case 1343: return "SixAxisForceTorqueSensor_setSecondLinkSensorTransform";
-  case 1344: return "SixAxisForceTorqueSensor_getFirstLinkIndex";
-  case 1345: return "SixAxisForceTorqueSensor_getSecondLinkIndex";
-  case 1346: return "SixAxisForceTorqueSensor_setFirstLinkName";
-  case 1347: return "SixAxisForceTorqueSensor_setSecondLinkName";
-  case 1348: return "SixAxisForceTorqueSensor_getFirstLinkName";
-  case 1349: return "SixAxisForceTorqueSensor_getSecondLinkName";
-  case 1350: return "SixAxisForceTorqueSensor_setParentJoint";
-  case 1351: return "SixAxisForceTorqueSensor_setParentJointIndex";
-  case 1352: return "SixAxisForceTorqueSensor_setAppliedWrenchLink";
-  case 1353: return "SixAxisForceTorqueSensor_getName";
-  case 1354: return "SixAxisForceTorqueSensor_getSensorType";
-  case 1355: return "SixAxisForceTorqueSensor_getParentJoint";
-  case 1356: return "SixAxisForceTorqueSensor_getParentJointIndex";
-  case 1357: return "SixAxisForceTorqueSensor_isValid";
-  case 1358: return "SixAxisForceTorqueSensor_clone";
-  case 1359: return "SixAxisForceTorqueSensor_updateIndices";
-  case 1360: return "SixAxisForceTorqueSensor_updateIndeces";
-  case 1361: return "SixAxisForceTorqueSensor_getAppliedWrenchLink";
-  case 1362: return "SixAxisForceTorqueSensor_isLinkAttachedToSensor";
-  case 1363: return "SixAxisForceTorqueSensor_getLinkSensorTransform";
-  case 1364: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLink";
-  case 1365: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix";
-  case 1366: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix";
-  case 1367: return "SixAxisForceTorqueSensor_predictMeasurement";
-  case 1368: return "SixAxisForceTorqueSensor_toString";
-  case 1369: return "new_AccelerometerSensor";
-  case 1370: return "delete_AccelerometerSensor";
-  case 1371: return "AccelerometerSensor_setName";
-  case 1372: return "AccelerometerSensor_setLinkSensorTransform";
-  case 1373: return "AccelerometerSensor_setParentLink";
-  case 1374: return "AccelerometerSensor_setParentLinkIndex";
-  case 1375: return "AccelerometerSensor_getName";
-  case 1376: return "AccelerometerSensor_getSensorType";
-  case 1377: return "AccelerometerSensor_getParentLink";
-  case 1378: return "AccelerometerSensor_getParentLinkIndex";
-  case 1379: return "AccelerometerSensor_getLinkSensorTransform";
-  case 1380: return "AccelerometerSensor_isValid";
-  case 1381: return "AccelerometerSensor_clone";
-  case 1382: return "AccelerometerSensor_updateIndices";
-  case 1383: return "AccelerometerSensor_updateIndeces";
-  case 1384: return "AccelerometerSensor_predictMeasurement";
-  case 1385: return "new_GyroscopeSensor";
-  case 1386: return "delete_GyroscopeSensor";
-  case 1387: return "GyroscopeSensor_setName";
-  case 1388: return "GyroscopeSensor_setLinkSensorTransform";
-  case 1389: return "GyroscopeSensor_setParentLink";
-  case 1390: return "GyroscopeSensor_setParentLinkIndex";
-  case 1391: return "GyroscopeSensor_getName";
-  case 1392: return "GyroscopeSensor_getSensorType";
-  case 1393: return "GyroscopeSensor_getParentLink";
-  case 1394: return "GyroscopeSensor_getParentLinkIndex";
-  case 1395: return "GyroscopeSensor_getLinkSensorTransform";
-  case 1396: return "GyroscopeSensor_isValid";
-  case 1397: return "GyroscopeSensor_clone";
-  case 1398: return "GyroscopeSensor_updateIndices";
-  case 1399: return "GyroscopeSensor_updateIndeces";
-  case 1400: return "GyroscopeSensor_predictMeasurement";
-  case 1401: return "new_ThreeAxisAngularAccelerometerSensor";
-  case 1402: return "delete_ThreeAxisAngularAccelerometerSensor";
-  case 1403: return "ThreeAxisAngularAccelerometerSensor_setName";
-  case 1404: return "ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform";
-  case 1405: return "ThreeAxisAngularAccelerometerSensor_setParentLink";
-  case 1406: return "ThreeAxisAngularAccelerometerSensor_setParentLinkIndex";
-  case 1407: return "ThreeAxisAngularAccelerometerSensor_getName";
-  case 1408: return "ThreeAxisAngularAccelerometerSensor_getSensorType";
-  case 1409: return "ThreeAxisAngularAccelerometerSensor_getParentLink";
-  case 1410: return "ThreeAxisAngularAccelerometerSensor_getParentLinkIndex";
-  case 1411: return "ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform";
-  case 1412: return "ThreeAxisAngularAccelerometerSensor_isValid";
-  case 1413: return "ThreeAxisAngularAccelerometerSensor_clone";
-  case 1414: return "ThreeAxisAngularAccelerometerSensor_updateIndices";
-  case 1415: return "ThreeAxisAngularAccelerometerSensor_predictMeasurement";
-  case 1416: return "new_ThreeAxisForceTorqueContactSensor";
-  case 1417: return "delete_ThreeAxisForceTorqueContactSensor";
-  case 1418: return "ThreeAxisForceTorqueContactSensor_setName";
-  case 1419: return "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform";
-  case 1420: return "ThreeAxisForceTorqueContactSensor_setParentLink";
-  case 1421: return "ThreeAxisForceTorqueContactSensor_setParentLinkIndex";
-  case 1422: return "ThreeAxisForceTorqueContactSensor_getName";
-  case 1423: return "ThreeAxisForceTorqueContactSensor_getSensorType";
-  case 1424: return "ThreeAxisForceTorqueContactSensor_getParentLink";
-  case 1425: return "ThreeAxisForceTorqueContactSensor_getParentLinkIndex";
-  case 1426: return "ThreeAxisForceTorqueContactSensor_getLinkSensorTransform";
-  case 1427: return "ThreeAxisForceTorqueContactSensor_isValid";
-  case 1428: return "ThreeAxisForceTorqueContactSensor_clone";
-  case 1429: return "ThreeAxisForceTorqueContactSensor_updateIndices";
-  case 1430: return "ThreeAxisForceTorqueContactSensor_setLoadCellLocations";
-  case 1431: return "ThreeAxisForceTorqueContactSensor_getLoadCellLocations";
-  case 1432: return "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements";
-  case 1433: return "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements";
-  case 1434: return "_wrap_predictSensorsMeasurements";
-  case 1435: return "_wrap_predictSensorsMeasurementsFromRawBuffers";
-  case 1436: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_get";
-  case 1437: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_set";
-  case 1438: return "URDFParserOptions_originalFilename_get";
-  case 1439: return "URDFParserOptions_originalFilename_set";
-  case 1440: return "new_URDFParserOptions";
-  case 1441: return "delete_URDFParserOptions";
-  case 1442: return "_wrap_modelFromURDF";
-  case 1443: return "_wrap_modelFromURDFString";
-  case 1444: return "_wrap_dofsListFromURDF";
-  case 1445: return "_wrap_dofsListFromURDFString";
-  case 1446: return "_wrap_sensorsFromURDF";
-  case 1447: return "_wrap_sensorsFromURDFString";
-  case 1448: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_get";
-  case 1449: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_set";
-  case 1450: return "ModelParserOptions_originalFilename_get";
-  case 1451: return "ModelParserOptions_originalFilename_set";
-  case 1452: return "new_ModelParserOptions";
-  case 1453: return "delete_ModelParserOptions";
-  case 1454: return "new_ModelLoader";
-  case 1455: return "delete_ModelLoader";
-  case 1456: return "ModelLoader_parsingOptions";
-  case 1457: return "ModelLoader_setParsingOptions";
-  case 1458: return "ModelLoader_loadModelFromString";
-  case 1459: return "ModelLoader_loadModelFromFile";
-  case 1460: return "ModelLoader_loadReducedModelFromFullModel";
-  case 1461: return "ModelLoader_loadReducedModelFromString";
-  case 1462: return "ModelLoader_loadReducedModelFromFile";
-  case 1463: return "ModelLoader_model";
-  case 1464: return "ModelLoader_sensors";
-  case 1465: return "ModelLoader_isValid";
-  case 1466: return "new_UnknownWrenchContact";
-  case 1467: return "UnknownWrenchContact_unknownType_get";
-  case 1468: return "UnknownWrenchContact_unknownType_set";
-  case 1469: return "UnknownWrenchContact_contactPoint_get";
-  case 1470: return "UnknownWrenchContact_contactPoint_set";
-  case 1471: return "UnknownWrenchContact_forceDirection_get";
-  case 1472: return "UnknownWrenchContact_forceDirection_set";
-  case 1473: return "UnknownWrenchContact_knownWrench_get";
-  case 1474: return "UnknownWrenchContact_knownWrench_set";
-  case 1475: return "UnknownWrenchContact_contactId_get";
-  case 1476: return "UnknownWrenchContact_contactId_set";
-  case 1477: return "delete_UnknownWrenchContact";
-  case 1478: return "new_LinkUnknownWrenchContacts";
-  case 1479: return "LinkUnknownWrenchContacts_clear";
-  case 1480: return "LinkUnknownWrenchContacts_resize";
-  case 1481: return "LinkUnknownWrenchContacts_getNrOfContactsForLink";
-  case 1482: return "LinkUnknownWrenchContacts_setNrOfContactsForLink";
-  case 1483: return "LinkUnknownWrenchContacts_addNewContactForLink";
-  case 1484: return "LinkUnknownWrenchContacts_addNewContactInFrame";
-  case 1485: return "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin";
-  case 1486: return "LinkUnknownWrenchContacts_contactWrench";
-  case 1487: return "LinkUnknownWrenchContacts_toString";
-  case 1488: return "delete_LinkUnknownWrenchContacts";
-  case 1489: return "new_estimateExternalWrenchesBuffers";
-  case 1490: return "estimateExternalWrenchesBuffers_resize";
-  case 1491: return "estimateExternalWrenchesBuffers_getNrOfSubModels";
-  case 1492: return "estimateExternalWrenchesBuffers_getNrOfLinks";
-  case 1493: return "estimateExternalWrenchesBuffers_isConsistent";
-  case 1494: return "estimateExternalWrenchesBuffers_A_get";
-  case 1495: return "estimateExternalWrenchesBuffers_A_set";
-  case 1496: return "estimateExternalWrenchesBuffers_x_get";
-  case 1497: return "estimateExternalWrenchesBuffers_x_set";
-  case 1498: return "estimateExternalWrenchesBuffers_b_get";
-  case 1499: return "estimateExternalWrenchesBuffers_b_set";
-  case 1500: return "estimateExternalWrenchesBuffers_pinvA_get";
-  case 1501: return "estimateExternalWrenchesBuffers_pinvA_set";
-  case 1502: return "estimateExternalWrenchesBuffers_b_contacts_subtree_get";
-  case 1503: return "estimateExternalWrenchesBuffers_b_contacts_subtree_set";
-  case 1504: return "estimateExternalWrenchesBuffers_subModelBase_H_link_get";
-  case 1505: return "estimateExternalWrenchesBuffers_subModelBase_H_link_set";
-  case 1506: return "delete_estimateExternalWrenchesBuffers";
-  case 1507: return "_wrap_estimateExternalWrenchesWithoutInternalFT";
-  case 1508: return "_wrap_estimateExternalWrenches";
-  case 1509: return "_wrap_dynamicsEstimationForwardVelAccKinematics";
-  case 1510: return "_wrap_dynamicsEstimationForwardVelKinematics";
-  case 1511: return "_wrap_computeLinkNetWrenchesWithoutGravity";
-  case 1512: return "_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches";
-  case 1513: return "new_ExtWrenchesAndJointTorquesEstimator";
-  case 1514: return "delete_ExtWrenchesAndJointTorquesEstimator";
-  case 1515: return "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors";
-  case 1516: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile";
-  case 1517: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs";
-  case 1518: return "ExtWrenchesAndJointTorquesEstimator_model";
-  case 1519: return "ExtWrenchesAndJointTorquesEstimator_sensors";
-  case 1520: return "ExtWrenchesAndJointTorquesEstimator_submodels";
-  case 1521: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase";
-  case 1522: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase";
-  case 1523: return "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements";
-  case 1524: return "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques";
-  case 1525: return "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill";
-  case 1526: return "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity";
-  case 1527: return "new_SimpleLeggedOdometry";
-  case 1528: return "delete_SimpleLeggedOdometry";
-  case 1529: return "SimpleLeggedOdometry_setModel";
-  case 1530: return "SimpleLeggedOdometry_loadModelFromFile";
-  case 1531: return "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs";
-  case 1532: return "SimpleLeggedOdometry_model";
-  case 1533: return "SimpleLeggedOdometry_updateKinematics";
-  case 1534: return "SimpleLeggedOdometry_init";
-  case 1535: return "SimpleLeggedOdometry_changeFixedFrame";
-  case 1536: return "SimpleLeggedOdometry_getCurrentFixedLink";
-  case 1537: return "SimpleLeggedOdometry_getWorldLinkTransform";
-  case 1538: return "_wrap_isLinkBerdyDynamicVariable";
-  case 1539: return "_wrap_isJointBerdyDynamicVariable";
-  case 1540: return "_wrap_isDOFBerdyDynamicVariable";
-  case 1541: return "new_BerdyOptions";
-  case 1542: return "BerdyOptions_berdyVariant_get";
-  case 1543: return "BerdyOptions_berdyVariant_set";
-  case 1544: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get";
-  case 1545: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set";
-  case 1546: return "BerdyOptions_includeAllJointAccelerationsAsSensors_get";
-  case 1547: return "BerdyOptions_includeAllJointAccelerationsAsSensors_set";
-  case 1548: return "BerdyOptions_includeAllJointTorquesAsSensors_get";
-  case 1549: return "BerdyOptions_includeAllJointTorquesAsSensors_set";
-  case 1550: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get";
-  case 1551: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set";
-  case 1552: return "BerdyOptions_includeFixedBaseExternalWrench_get";
-  case 1553: return "BerdyOptions_includeFixedBaseExternalWrench_set";
-  case 1554: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get";
-  case 1555: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set";
-  case 1556: return "BerdyOptions_baseLink_get";
-  case 1557: return "BerdyOptions_baseLink_set";
-  case 1558: return "BerdyOptions_checkConsistency";
-  case 1559: return "delete_BerdyOptions";
-  case 1560: return "BerdySensor_type_get";
-  case 1561: return "BerdySensor_type_set";
-  case 1562: return "BerdySensor_id_get";
-  case 1563: return "BerdySensor_id_set";
-  case 1564: return "BerdySensor_range_get";
-  case 1565: return "BerdySensor_range_set";
-  case 1566: return "BerdySensor_eq";
-  case 1567: return "BerdySensor_lt";
-  case 1568: return "new_BerdySensor";
-  case 1569: return "delete_BerdySensor";
-  case 1570: return "BerdyDynamicVariable_type_get";
-  case 1571: return "BerdyDynamicVariable_type_set";
-  case 1572: return "BerdyDynamicVariable_id_get";
-  case 1573: return "BerdyDynamicVariable_id_set";
-  case 1574: return "BerdyDynamicVariable_range_get";
-  case 1575: return "BerdyDynamicVariable_range_set";
-  case 1576: return "BerdyDynamicVariable_eq";
-  case 1577: return "BerdyDynamicVariable_lt";
-  case 1578: return "new_BerdyDynamicVariable";
-  case 1579: return "delete_BerdyDynamicVariable";
-  case 1580: return "new_BerdyHelper";
-  case 1581: return "BerdyHelper_dynamicTraversal";
-  case 1582: return "BerdyHelper_model";
-  case 1583: return "BerdyHelper_sensors";
-  case 1584: return "BerdyHelper_isValid";
-  case 1585: return "BerdyHelper_init";
-  case 1586: return "BerdyHelper_getOptions";
-  case 1587: return "BerdyHelper_getNrOfDynamicVariables";
-  case 1588: return "BerdyHelper_getNrOfDynamicEquations";
-  case 1589: return "BerdyHelper_getNrOfSensorsMeasurements";
-  case 1590: return "BerdyHelper_resizeAndZeroBerdyMatrices";
-  case 1591: return "BerdyHelper_getBerdyMatrices";
-  case 1592: return "BerdyHelper_getSensorsOrdering";
-  case 1593: return "BerdyHelper_getRangeSensorVariable";
-  case 1594: return "BerdyHelper_getRangeDOFSensorVariable";
-  case 1595: return "BerdyHelper_getRangeJointSensorVariable";
-  case 1596: return "BerdyHelper_getRangeLinkSensorVariable";
-  case 1597: return "BerdyHelper_getRangeLinkVariable";
-  case 1598: return "BerdyHelper_getRangeJointVariable";
-  case 1599: return "BerdyHelper_getRangeDOFVariable";
-  case 1600: return "BerdyHelper_getDynamicVariablesOrdering";
-  case 1601: return "BerdyHelper_serializeDynamicVariables";
-  case 1602: return "BerdyHelper_serializeSensorVariables";
-  case 1603: return "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA";
-  case 1604: return "BerdyHelper_extractJointTorquesFromDynamicVariables";
-  case 1605: return "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables";
-  case 1606: return "BerdyHelper_updateKinematicsFromFloatingBase";
-  case 1607: return "BerdyHelper_updateKinematicsFromFixedBase";
-  case 1608: return "BerdyHelper_updateKinematicsFromTraversalFixedBase";
-  case 1609: return "BerdyHelper_setNetExternalWrenchMeasurementFrame";
-  case 1610: return "BerdyHelper_getNetExternalWrenchMeasurementFrame";
-  case 1611: return "delete_BerdyHelper";
-  case 1612: return "new_BerdySparseMAPSolver";
-  case 1613: return "delete_BerdySparseMAPSolver";
-  case 1614: return "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance";
-  case 1615: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance";
-  case 1616: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue";
-  case 1617: return "BerdySparseMAPSolver_setMeasurementsPriorCovariance";
-  case 1618: return "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse";
-  case 1619: return "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse";
-  case 1620: return "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue";
-  case 1621: return "BerdySparseMAPSolver_measurementsPriorCovarianceInverse";
-  case 1622: return "BerdySparseMAPSolver_isValid";
-  case 1623: return "BerdySparseMAPSolver_initialize";
-  case 1624: return "BerdySparseMAPSolver_updateEstimateInformationFixedBase";
-  case 1625: return "BerdySparseMAPSolver_updateEstimateInformationFloatingBase";
-  case 1626: return "BerdySparseMAPSolver_doEstimate";
-  case 1627: return "BerdySparseMAPSolver_getLastEstimate";
-  case 1628: return "delete_IAttitudeEstimator";
-  case 1629: return "IAttitudeEstimator_updateFilterWithMeasurements";
-  case 1630: return "IAttitudeEstimator_propagateStates";
-  case 1631: return "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix";
-  case 1632: return "IAttitudeEstimator_getOrientationEstimateAsQuaternion";
-  case 1633: return "IAttitudeEstimator_getOrientationEstimateAsRPY";
-  case 1634: return "IAttitudeEstimator_getInternalStateSize";
-  case 1635: return "IAttitudeEstimator_getInternalState";
-  case 1636: return "IAttitudeEstimator_getDefaultInternalInitialState";
-  case 1637: return "IAttitudeEstimator_setInternalState";
-  case 1638: return "IAttitudeEstimator_setInternalStateInitialOrientation";
-  case 1639: return "AttitudeMahonyFilterParameters_time_step_in_seconds_get";
-  case 1640: return "AttitudeMahonyFilterParameters_time_step_in_seconds_set";
-  case 1641: return "AttitudeMahonyFilterParameters_kp_get";
-  case 1642: return "AttitudeMahonyFilterParameters_kp_set";
-  case 1643: return "AttitudeMahonyFilterParameters_ki_get";
-  case 1644: return "AttitudeMahonyFilterParameters_ki_set";
-  case 1645: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_get";
-  case 1646: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set";
-  case 1647: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get";
-  case 1648: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set";
-  case 1649: return "new_AttitudeMahonyFilterParameters";
-  case 1650: return "delete_AttitudeMahonyFilterParameters";
-  case 1651: return "new_AttitudeMahonyFilter";
-  case 1652: return "AttitudeMahonyFilter_useMagnetoMeterMeasurements";
-  case 1653: return "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements";
-  case 1654: return "AttitudeMahonyFilter_setGainkp";
-  case 1655: return "AttitudeMahonyFilter_setGainki";
-  case 1656: return "AttitudeMahonyFilter_setTimeStepInSeconds";
-  case 1657: return "AttitudeMahonyFilter_setGravityDirection";
-  case 1658: return "AttitudeMahonyFilter_setParameters";
-  case 1659: return "AttitudeMahonyFilter_getParameters";
-  case 1660: return "AttitudeMahonyFilter_updateFilterWithMeasurements";
-  case 1661: return "AttitudeMahonyFilter_propagateStates";
-  case 1662: return "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix";
-  case 1663: return "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion";
-  case 1664: return "AttitudeMahonyFilter_getOrientationEstimateAsRPY";
-  case 1665: return "AttitudeMahonyFilter_getInternalStateSize";
-  case 1666: return "AttitudeMahonyFilter_getInternalState";
-  case 1667: return "AttitudeMahonyFilter_getDefaultInternalInitialState";
-  case 1668: return "AttitudeMahonyFilter_setInternalState";
-  case 1669: return "AttitudeMahonyFilter_setInternalStateInitialOrientation";
-  case 1670: return "delete_AttitudeMahonyFilter";
-  case 1671: return "DiscreteExtendedKalmanFilterHelper_ekf_f";
-  case 1672: return "DiscreteExtendedKalmanFilterHelper_ekf_h";
-  case 1673: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF";
-  case 1674: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH";
-  case 1675: return "DiscreteExtendedKalmanFilterHelper_ekfPredict";
-  case 1676: return "DiscreteExtendedKalmanFilterHelper_ekfUpdate";
-  case 1677: return "DiscreteExtendedKalmanFilterHelper_ekfInit";
-  case 1678: return "DiscreteExtendedKalmanFilterHelper_ekfReset";
-  case 1679: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector";
-  case 1680: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector";
-  case 1681: return "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState";
-  case 1682: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance";
-  case 1683: return "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance";
-  case 1684: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance";
-  case 1685: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize";
-  case 1686: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize";
-  case 1687: return "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize";
-  case 1688: return "DiscreteExtendedKalmanFilterHelper_ekfGetStates";
-  case 1689: return "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance";
-  case 1690: return "delete_DiscreteExtendedKalmanFilterHelper";
-  case 1691: return "output_dimensions_with_magnetometer_get";
-  case 1692: return "output_dimensions_without_magnetometer_get";
-  case 1693: return "input_dimensions_get";
-  case 1694: return "AttitudeEstimatorState_m_orientation_get";
-  case 1695: return "AttitudeEstimatorState_m_orientation_set";
-  case 1696: return "AttitudeEstimatorState_m_angular_velocity_get";
-  case 1697: return "AttitudeEstimatorState_m_angular_velocity_set";
-  case 1698: return "AttitudeEstimatorState_m_gyroscope_bias_get";
-  case 1699: return "AttitudeEstimatorState_m_gyroscope_bias_set";
-  case 1700: return "new_AttitudeEstimatorState";
-  case 1701: return "delete_AttitudeEstimatorState";
-  case 1702: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_get";
-  case 1703: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_set";
-  case 1704: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get";
-  case 1705: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set";
-  case 1706: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get";
-  case 1707: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set";
-  case 1708: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get";
-  case 1709: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set";
-  case 1710: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get";
-  case 1711: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set";
-  case 1712: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get";
-  case 1713: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set";
-  case 1714: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get";
-  case 1715: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set";
-  case 1716: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get";
-  case 1717: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set";
-  case 1718: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get";
-  case 1719: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set";
-  case 1720: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get";
-  case 1721: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set";
-  case 1722: return "new_AttitudeQuaternionEKFParameters";
-  case 1723: return "delete_AttitudeQuaternionEKFParameters";
-  case 1724: return "new_AttitudeQuaternionEKF";
-  case 1725: return "AttitudeQuaternionEKF_getParameters";
-  case 1726: return "AttitudeQuaternionEKF_setParameters";
-  case 1727: return "AttitudeQuaternionEKF_setGravityDirection";
-  case 1728: return "AttitudeQuaternionEKF_setTimeStepInSeconds";
-  case 1729: return "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor";
-  case 1730: return "AttitudeQuaternionEKF_useMagnetometerMeasurements";
-  case 1731: return "AttitudeQuaternionEKF_setMeasurementNoiseVariance";
-  case 1732: return "AttitudeQuaternionEKF_setSystemNoiseVariance";
-  case 1733: return "AttitudeQuaternionEKF_setInitialStateCovariance";
-  case 1734: return "AttitudeQuaternionEKF_initializeFilter";
-  case 1735: return "AttitudeQuaternionEKF_updateFilterWithMeasurements";
-  case 1736: return "AttitudeQuaternionEKF_propagateStates";
-  case 1737: return "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix";
-  case 1738: return "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion";
-  case 1739: return "AttitudeQuaternionEKF_getOrientationEstimateAsRPY";
-  case 1740: return "AttitudeQuaternionEKF_getInternalStateSize";
-  case 1741: return "AttitudeQuaternionEKF_getInternalState";
-  case 1742: return "AttitudeQuaternionEKF_getDefaultInternalInitialState";
-  case 1743: return "AttitudeQuaternionEKF_setInternalState";
-  case 1744: return "AttitudeQuaternionEKF_setInternalStateInitialOrientation";
-  case 1745: return "delete_AttitudeQuaternionEKF";
-  case 1746: return "DynamicsRegressorParameter_category_get";
-  case 1747: return "DynamicsRegressorParameter_category_set";
-  case 1748: return "DynamicsRegressorParameter_elemIndex_get";
-  case 1749: return "DynamicsRegressorParameter_elemIndex_set";
-  case 1750: return "DynamicsRegressorParameter_type_get";
-  case 1751: return "DynamicsRegressorParameter_type_set";
-  case 1752: return "DynamicsRegressorParameter_lt";
-  case 1753: return "DynamicsRegressorParameter_eq";
-  case 1754: return "DynamicsRegressorParameter_ne";
-  case 1755: return "new_DynamicsRegressorParameter";
-  case 1756: return "delete_DynamicsRegressorParameter";
-  case 1757: return "DynamicsRegressorParametersList_parameters_get";
-  case 1758: return "DynamicsRegressorParametersList_parameters_set";
-  case 1759: return "DynamicsRegressorParametersList_getDescriptionOfParameter";
-  case 1760: return "DynamicsRegressorParametersList_addParam";
-  case 1761: return "DynamicsRegressorParametersList_addList";
-  case 1762: return "DynamicsRegressorParametersList_findParam";
-  case 1763: return "DynamicsRegressorParametersList_getNrOfParameters";
-  case 1764: return "new_DynamicsRegressorParametersList";
-  case 1765: return "delete_DynamicsRegressorParametersList";
-  case 1766: return "new_DynamicsRegressorGenerator";
-  case 1767: return "delete_DynamicsRegressorGenerator";
-  case 1768: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile";
-  case 1769: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString";
-  case 1770: return "DynamicsRegressorGenerator_loadRegressorStructureFromFile";
-  case 1771: return "DynamicsRegressorGenerator_loadRegressorStructureFromString";
-  case 1772: return "DynamicsRegressorGenerator_isValid";
-  case 1773: return "DynamicsRegressorGenerator_getNrOfParameters";
-  case 1774: return "DynamicsRegressorGenerator_getNrOfOutputs";
-  case 1775: return "DynamicsRegressorGenerator_getNrOfDegreesOfFreedom";
-  case 1776: return "DynamicsRegressorGenerator_getDescriptionOfParameter";
-  case 1777: return "DynamicsRegressorGenerator_getDescriptionOfParameters";
-  case 1778: return "DynamicsRegressorGenerator_getDescriptionOfOutput";
-  case 1779: return "DynamicsRegressorGenerator_getDescriptionOfOutputs";
-  case 1780: return "DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom";
-  case 1781: return "DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom";
-  case 1782: return "DynamicsRegressorGenerator_getDescriptionOfLink";
-  case 1783: return "DynamicsRegressorGenerator_getDescriptionOfLinks";
-  case 1784: return "DynamicsRegressorGenerator_getNrOfLinks";
-  case 1785: return "DynamicsRegressorGenerator_getNrOfFakeLinks";
-  case 1786: return "DynamicsRegressorGenerator_getBaseLinkName";
-  case 1787: return "DynamicsRegressorGenerator_getSensorsModel";
-  case 1788: return "DynamicsRegressorGenerator_setRobotState";
-  case 1789: return "DynamicsRegressorGenerator_getSensorsMeasurements";
-  case 1790: return "DynamicsRegressorGenerator_setTorqueSensorMeasurement";
-  case 1791: return "DynamicsRegressorGenerator_computeRegressor";
-  case 1792: return "DynamicsRegressorGenerator_getModelParameters";
-  case 1793: return "DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace";
-  case 1794: return "DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace";
-  case 1795: return "DynamicsRegressorGenerator_generate_random_regressors";
-  case 1796: return "new_KinDynComputations";
-  case 1797: return "delete_KinDynComputations";
-  case 1798: return "KinDynComputations_loadRobotModel";
-  case 1799: return "KinDynComputations_loadRobotModelFromFile";
-  case 1800: return "KinDynComputations_loadRobotModelFromString";
-  case 1801: return "KinDynComputations_isValid";
-  case 1802: return "KinDynComputations_setFrameVelocityRepresentation";
-  case 1803: return "KinDynComputations_getFrameVelocityRepresentation";
-  case 1804: return "KinDynComputations_getNrOfDegreesOfFreedom";
-  case 1805: return "KinDynComputations_getDescriptionOfDegreeOfFreedom";
-  case 1806: return "KinDynComputations_getDescriptionOfDegreesOfFreedom";
-  case 1807: return "KinDynComputations_getNrOfLinks";
-  case 1808: return "KinDynComputations_getNrOfFrames";
-  case 1809: return "KinDynComputations_getFloatingBase";
-  case 1810: return "KinDynComputations_setFloatingBase";
-  case 1811: return "KinDynComputations_model";
-  case 1812: return "KinDynComputations_getRobotModel";
-  case 1813: return "KinDynComputations_getRelativeJacobianSparsityPattern";
-  case 1814: return "KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern";
-  case 1815: return "KinDynComputations_setJointPos";
-  case 1816: return "KinDynComputations_setRobotState";
-  case 1817: return "KinDynComputations_getRobotState";
-  case 1818: return "KinDynComputations_getWorldBaseTransform";
-  case 1819: return "KinDynComputations_getBaseTwist";
-  case 1820: return "KinDynComputations_getJointPos";
-  case 1821: return "KinDynComputations_getJointVel";
-  case 1822: return "KinDynComputations_getModelVel";
-  case 1823: return "KinDynComputations_getFrameIndex";
-  case 1824: return "KinDynComputations_getFrameName";
-  case 1825: return "KinDynComputations_getWorldTransform";
-  case 1826: return "KinDynComputations_getRelativeTransformExplicit";
-  case 1827: return "KinDynComputations_getRelativeTransform";
-  case 1828: return "KinDynComputations_getFrameVel";
-  case 1829: return "KinDynComputations_getFrameAcc";
-  case 1830: return "KinDynComputations_getFrameFreeFloatingJacobian";
-  case 1831: return "KinDynComputations_getRelativeJacobian";
-  case 1832: return "KinDynComputations_getRelativeJacobianExplicit";
-  case 1833: return "KinDynComputations_getFrameBiasAcc";
-  case 1834: return "KinDynComputations_getCenterOfMassPosition";
-  case 1835: return "KinDynComputations_getCenterOfMassVelocity";
-  case 1836: return "KinDynComputations_getCenterOfMassJacobian";
-  case 1837: return "KinDynComputations_getCenterOfMassBiasAcc";
-  case 1838: return "KinDynComputations_getAverageVelocity";
-  case 1839: return "KinDynComputations_getAverageVelocityJacobian";
-  case 1840: return "KinDynComputations_getCentroidalAverageVelocity";
-  case 1841: return "KinDynComputations_getCentroidalAverageVelocityJacobian";
-  case 1842: return "KinDynComputations_getLinearAngularMomentum";
-  case 1843: return "KinDynComputations_getLinearAngularMomentumJacobian";
-  case 1844: return "KinDynComputations_getCentroidalTotalMomentum";
-  case 1845: return "KinDynComputations_getFreeFloatingMassMatrix";
-  case 1846: return "KinDynComputations_inverseDynamics";
-  case 1847: return "KinDynComputations_generalizedBiasForces";
-  case 1848: return "KinDynComputations_generalizedGravityForces";
-  case 1849: return "KinDynComputations_generalizedExternalForces";
-  case 1850: return "KinDynComputations_inverseDynamicsInertialParametersRegressor";
-  case 1851: return "delete_ICamera";
-  case 1852: return "ICamera_setPosition";
-  case 1853: return "ICamera_setTarget";
-  case 1854: return "ICamera_setUpVector";
-  case 1855: return "ColorViz_r_get";
-  case 1856: return "ColorViz_r_set";
-  case 1857: return "ColorViz_g_get";
-  case 1858: return "ColorViz_g_set";
-  case 1859: return "ColorViz_b_get";
-  case 1860: return "ColorViz_b_set";
-  case 1861: return "ColorViz_a_get";
-  case 1862: return "ColorViz_a_set";
-  case 1863: return "new_ColorViz";
-  case 1864: return "delete_ColorViz";
-  case 1865: return "delete_ILight";
-  case 1866: return "ILight_getName";
-  case 1867: return "ILight_setType";
-  case 1868: return "ILight_getType";
-  case 1869: return "ILight_setPosition";
-  case 1870: return "ILight_getPosition";
-  case 1871: return "ILight_setDirection";
-  case 1872: return "ILight_getDirection";
-  case 1873: return "ILight_setAmbientColor";
-  case 1874: return "ILight_getAmbientColor";
-  case 1875: return "ILight_setSpecularColor";
-  case 1876: return "ILight_getSpecularColor";
-  case 1877: return "ILight_setDiffuseColor";
-  case 1878: return "ILight_getDiffuseColor";
-  case 1879: return "delete_IEnvironment";
-  case 1880: return "IEnvironment_getElements";
-  case 1881: return "IEnvironment_setElementVisibility";
-  case 1882: return "IEnvironment_setBackgroundColor";
-  case 1883: return "IEnvironment_setAmbientLight";
-  case 1884: return "IEnvironment_getLights";
-  case 1885: return "IEnvironment_addLight";
-  case 1886: return "IEnvironment_lightViz";
-  case 1887: return "IEnvironment_removeLight";
-  case 1888: return "delete_IJetsVisualization";
-  case 1889: return "IJetsVisualization_setJetsFrames";
-  case 1890: return "IJetsVisualization_getNrOfJets";
-  case 1891: return "IJetsVisualization_getJetDirection";
-  case 1892: return "IJetsVisualization_setJetDirection";
-  case 1893: return "IJetsVisualization_setJetColor";
-  case 1894: return "IJetsVisualization_setJetsDimensions";
-  case 1895: return "IJetsVisualization_setJetsIntensity";
-  case 1896: return "delete_IVectorsVisualization";
-  case 1897: return "IVectorsVisualization_addVector";
-  case 1898: return "IVectorsVisualization_getNrOfVectors";
-  case 1899: return "IVectorsVisualization_getVector";
-  case 1900: return "IVectorsVisualization_updateVector";
-  case 1901: return "IVectorsVisualization_setVectorColor";
-  case 1902: return "IVectorsVisualization_setVectorsAspect";
-  case 1903: return "delete_IModelVisualization";
-  case 1904: return "IModelVisualization_setPositions";
-  case 1905: return "IModelVisualization_setLinkPositions";
-  case 1906: return "IModelVisualization_model";
-  case 1907: return "IModelVisualization_getInstanceName";
-  case 1908: return "IModelVisualization_setModelVisibility";
-  case 1909: return "IModelVisualization_setModelColor";
-  case 1910: return "IModelVisualization_resetModelColor";
-  case 1911: return "IModelVisualization_setLinkColor";
-  case 1912: return "IModelVisualization_resetLinkColor";
-  case 1913: return "IModelVisualization_getLinkNames";
-  case 1914: return "IModelVisualization_setLinkVisibility";
-  case 1915: return "IModelVisualization_getFeatures";
-  case 1916: return "IModelVisualization_setFeatureVisibility";
-  case 1917: return "IModelVisualization_jets";
-  case 1918: return "IModelVisualization_getWorldModelTransform";
-  case 1919: return "IModelVisualization_getWorldLinkTransform";
-  case 1920: return "VisualizerOptions_verbose_get";
-  case 1921: return "VisualizerOptions_verbose_set";
-  case 1922: return "VisualizerOptions_winWidth_get";
-  case 1923: return "VisualizerOptions_winWidth_set";
-  case 1924: return "VisualizerOptions_winHeight_get";
-  case 1925: return "VisualizerOptions_winHeight_set";
-  case 1926: return "VisualizerOptions_rootFrameArrowsDimension_get";
-  case 1927: return "VisualizerOptions_rootFrameArrowsDimension_set";
-  case 1928: return "new_VisualizerOptions";
-  case 1929: return "delete_VisualizerOptions";
-  case 1930: return "new_Visualizer";
-  case 1931: return "delete_Visualizer";
-  case 1932: return "Visualizer_init";
-  case 1933: return "Visualizer_getNrOfVisualizedModels";
-  case 1934: return "Visualizer_getModelInstanceName";
-  case 1935: return "Visualizer_getModelInstanceIndex";
-  case 1936: return "Visualizer_addModel";
-  case 1937: return "Visualizer_modelViz";
-  case 1938: return "Visualizer_camera";
-  case 1939: return "Visualizer_enviroment";
-  case 1940: return "Visualizer_vectors";
-  case 1941: return "Visualizer_run";
-  case 1942: return "Visualizer_draw";
-  case 1943: return "Visualizer_drawToFile";
-  case 1944: return "Visualizer_close";
-  case 1945: return "new_DynamicsComputations";
-  case 1946: return "delete_DynamicsComputations";
-  case 1947: return "DynamicsComputations_loadRobotModelFromFile";
-  case 1948: return "DynamicsComputations_loadRobotModelFromString";
-  case 1949: return "DynamicsComputations_isValid";
-  case 1950: return "DynamicsComputations_getNrOfDegreesOfFreedom";
-  case 1951: return "DynamicsComputations_getDescriptionOfDegreeOfFreedom";
-  case 1952: return "DynamicsComputations_getDescriptionOfDegreesOfFreedom";
-  case 1953: return "DynamicsComputations_getNrOfLinks";
-  case 1954: return "DynamicsComputations_getNrOfFrames";
-  case 1955: return "DynamicsComputations_getFloatingBase";
-  case 1956: return "DynamicsComputations_setFloatingBase";
-  case 1957: return "DynamicsComputations_setRobotState";
-  case 1958: return "DynamicsComputations_getWorldBaseTransform";
-  case 1959: return "DynamicsComputations_getBaseTwist";
-  case 1960: return "DynamicsComputations_getJointPos";
-  case 1961: return "DynamicsComputations_getJointVel";
-  case 1962: return "DynamicsComputations_getFrameIndex";
-  case 1963: return "DynamicsComputations_getFrameName";
-  case 1964: return "DynamicsComputations_getWorldTransform";
-  case 1965: return "DynamicsComputations_getRelativeTransform";
-  case 1966: return "DynamicsComputations_getFrameTwist";
-  case 1967: return "DynamicsComputations_getFrameTwistInWorldOrient";
-  case 1968: return "DynamicsComputations_getFrameProperSpatialAcceleration";
-  case 1969: return "DynamicsComputations_getLinkIndex";
-  case 1970: return "DynamicsComputations_getLinkInertia";
-  case 1971: return "DynamicsComputations_getJointIndex";
-  case 1972: return "DynamicsComputations_getJointName";
-  case 1973: return "DynamicsComputations_getJointLimits";
-  case 1974: return "DynamicsComputations_inverseDynamics";
-  case 1975: return "DynamicsComputations_getFreeFloatingMassMatrix";
-  case 1976: return "DynamicsComputations_getFrameJacobian";
-  case 1977: return "DynamicsComputations_getDynamicsRegressor";
-  case 1978: return "DynamicsComputations_getModelDynamicsParameters";
-  case 1979: return "DynamicsComputations_getCenterOfMass";
-  case 1980: return "DynamicsComputations_getCenterOfMassJacobian";
+  case 768: return "Rotation_RPYRightTrivializedDerivativeRateOfChange";
+  case 769: return "Rotation_RPYRightTrivializedDerivativeInverse";
+  case 770: return "Rotation_RPYRightTrivializedDerivativeInverseRateOfChange";
+  case 771: return "Rotation_QuaternionRightTrivializedDerivative";
+  case 772: return "Rotation_QuaternionRightTrivializedDerivativeInverse";
+  case 773: return "Rotation_Identity";
+  case 774: return "Rotation_RotationFromQuaternion";
+  case 775: return "Rotation_leftJacobian";
+  case 776: return "Rotation_leftJacobianInverse";
+  case 777: return "Rotation_toString";
+  case 778: return "Rotation_display";
+  case 779: return "delete_Rotation";
+  case 780: return "new_TransformSemantics";
+  case 781: return "TransformSemantics_getRotationSemantics";
+  case 782: return "TransformSemantics_getPositionSemantics";
+  case 783: return "TransformSemantics_setRotationSemantics";
+  case 784: return "TransformSemantics_setPositionSemantics";
+  case 785: return "TransformSemantics_toString";
+  case 786: return "TransformSemantics_display";
+  case 787: return "delete_TransformSemantics";
+  case 788: return "new_Transform";
+  case 789: return "Transform_fromHomogeneousTransform";
+  case 790: return "Transform_getSemantics";
+  case 791: return "Transform_getRotation";
+  case 792: return "Transform_getPosition";
+  case 793: return "Transform_setRotation";
+  case 794: return "Transform_setPosition";
+  case 795: return "Transform_compose";
+  case 796: return "Transform_inverse2";
+  case 797: return "Transform_inverse";
+  case 798: return "Transform_mtimes";
+  case 799: return "Transform_Identity";
+  case 800: return "Transform_asHomogeneousTransform";
+  case 801: return "Transform_asAdjointTransform";
+  case 802: return "Transform_asAdjointTransformWrench";
+  case 803: return "Transform_log";
+  case 804: return "Transform_toString";
+  case 805: return "Transform_display";
+  case 806: return "delete_Transform";
+  case 807: return "new_TransformDerivative";
+  case 808: return "delete_TransformDerivative";
+  case 809: return "TransformDerivative_getRotationDerivative";
+  case 810: return "TransformDerivative_getPositionDerivative";
+  case 811: return "TransformDerivative_setRotationDerivative";
+  case 812: return "TransformDerivative_setPositionDerivative";
+  case 813: return "TransformDerivative_Zero";
+  case 814: return "TransformDerivative_asHomogeneousTransformDerivative";
+  case 815: return "TransformDerivative_asAdjointTransformDerivative";
+  case 816: return "TransformDerivative_asAdjointTransformWrenchDerivative";
+  case 817: return "TransformDerivative_mtimes";
+  case 818: return "TransformDerivative_derivativeOfInverse";
+  case 819: return "TransformDerivative_transform";
+  case 820: return "dynamic_extent_get";
+  case 821: return "DynamicSpan_extent_get";
+  case 822: return "new_DynamicSpan";
+  case 823: return "delete_DynamicSpan";
+  case 824: return "DynamicSpan_first";
+  case 825: return "DynamicSpan_last";
+  case 826: return "DynamicSpan_subspan";
+  case 827: return "DynamicSpan_size";
+  case 828: return "DynamicSpan_size_bytes";
+  case 829: return "DynamicSpan_empty";
+  case 830: return "DynamicSpan_brace";
+  case 831: return "DynamicSpan_getVal";
+  case 832: return "DynamicSpan_setVal";
+  case 833: return "DynamicSpan_at";
+  case 834: return "DynamicSpan_paren";
+  case 835: return "DynamicSpan_begin";
+  case 836: return "DynamicSpan_end";
+  case 837: return "DynamicSpan_cbegin";
+  case 838: return "DynamicSpan_cend";
+  case 839: return "DynamicSpan_rbegin";
+  case 840: return "DynamicSpan_rend";
+  case 841: return "DynamicSpan_crbegin";
+  case 842: return "DynamicSpan_crend";
+  case 843: return "LINK_INVALID_INDEX_get";
+  case 844: return "LINK_INVALID_INDEX_set";
+  case 845: return "LINK_INVALID_NAME_get";
+  case 846: return "LINK_INVALID_NAME_set";
+  case 847: return "JOINT_INVALID_INDEX_get";
+  case 848: return "JOINT_INVALID_INDEX_set";
+  case 849: return "JOINT_INVALID_NAME_get";
+  case 850: return "JOINT_INVALID_NAME_set";
+  case 851: return "DOF_INVALID_INDEX_get";
+  case 852: return "DOF_INVALID_INDEX_set";
+  case 853: return "DOF_INVALID_NAME_get";
+  case 854: return "DOF_INVALID_NAME_set";
+  case 855: return "FRAME_INVALID_INDEX_get";
+  case 856: return "FRAME_INVALID_INDEX_set";
+  case 857: return "FRAME_INVALID_NAME_get";
+  case 858: return "FRAME_INVALID_NAME_set";
+  case 859: return "TRAVERSAL_INVALID_INDEX_get";
+  case 860: return "TRAVERSAL_INVALID_INDEX_set";
+  case 861: return "new_LinkPositions";
+  case 862: return "LinkPositions_resize";
+  case 863: return "LinkPositions_isConsistent";
+  case 864: return "LinkPositions_getNrOfLinks";
+  case 865: return "LinkPositions_paren";
+  case 866: return "LinkPositions_toString";
+  case 867: return "delete_LinkPositions";
+  case 868: return "new_LinkWrenches";
+  case 869: return "LinkWrenches_resize";
+  case 870: return "LinkWrenches_isConsistent";
+  case 871: return "LinkWrenches_getNrOfLinks";
+  case 872: return "LinkWrenches_paren";
+  case 873: return "LinkWrenches_toString";
+  case 874: return "LinkWrenches_zero";
+  case 875: return "delete_LinkWrenches";
+  case 876: return "new_LinkInertias";
+  case 877: return "LinkInertias_resize";
+  case 878: return "LinkInertias_isConsistent";
+  case 879: return "LinkInertias_paren";
+  case 880: return "delete_LinkInertias";
+  case 881: return "new_LinkArticulatedBodyInertias";
+  case 882: return "LinkArticulatedBodyInertias_resize";
+  case 883: return "LinkArticulatedBodyInertias_isConsistent";
+  case 884: return "LinkArticulatedBodyInertias_paren";
+  case 885: return "delete_LinkArticulatedBodyInertias";
+  case 886: return "new_LinkVelArray";
+  case 887: return "LinkVelArray_resize";
+  case 888: return "LinkVelArray_isConsistent";
+  case 889: return "LinkVelArray_getNrOfLinks";
+  case 890: return "LinkVelArray_paren";
+  case 891: return "LinkVelArray_toString";
+  case 892: return "delete_LinkVelArray";
+  case 893: return "new_LinkAccArray";
+  case 894: return "LinkAccArray_resize";
+  case 895: return "LinkAccArray_isConsistent";
+  case 896: return "LinkAccArray_paren";
+  case 897: return "LinkAccArray_getNrOfLinks";
+  case 898: return "LinkAccArray_toString";
+  case 899: return "delete_LinkAccArray";
+  case 900: return "new_Link";
+  case 901: return "Link_inertia";
+  case 902: return "Link_setInertia";
+  case 903: return "Link_getInertia";
+  case 904: return "Link_setIndex";
+  case 905: return "Link_getIndex";
+  case 906: return "delete_Link";
+  case 907: return "delete_IJoint";
+  case 908: return "IJoint_clone";
+  case 909: return "IJoint_getNrOfPosCoords";
+  case 910: return "IJoint_getNrOfDOFs";
+  case 911: return "IJoint_setAttachedLinks";
+  case 912: return "IJoint_setRestTransform";
+  case 913: return "IJoint_getFirstAttachedLink";
+  case 914: return "IJoint_getSecondAttachedLink";
+  case 915: return "IJoint_getRestTransform";
+  case 916: return "IJoint_getTransform";
+  case 917: return "IJoint_getTransformDerivative";
+  case 918: return "IJoint_getMotionSubspaceVector";
+  case 919: return "IJoint_computeChildPosVelAcc";
+  case 920: return "IJoint_computeChildVelAcc";
+  case 921: return "IJoint_computeChildVel";
+  case 922: return "IJoint_computeChildAcc";
+  case 923: return "IJoint_computeChildBiasAcc";
+  case 924: return "IJoint_computeJointTorque";
+  case 925: return "IJoint_setIndex";
+  case 926: return "IJoint_getIndex";
+  case 927: return "IJoint_setPosCoordsOffset";
+  case 928: return "IJoint_getPosCoordsOffset";
+  case 929: return "IJoint_setDOFsOffset";
+  case 930: return "IJoint_getDOFsOffset";
+  case 931: return "IJoint_hasPosLimits";
+  case 932: return "IJoint_enablePosLimits";
+  case 933: return "IJoint_getPosLimits";
+  case 934: return "IJoint_getMinPosLimit";
+  case 935: return "IJoint_getMaxPosLimit";
+  case 936: return "IJoint_setPosLimits";
+  case 937: return "IJoint_isRevoluteJoint";
+  case 938: return "IJoint_isFixedJoint";
+  case 939: return "IJoint_asRevoluteJoint";
+  case 940: return "IJoint_asFixedJoint";
+  case 941: return "new_FixedJoint";
+  case 942: return "delete_FixedJoint";
+  case 943: return "FixedJoint_clone";
+  case 944: return "FixedJoint_getNrOfPosCoords";
+  case 945: return "FixedJoint_getNrOfDOFs";
+  case 946: return "FixedJoint_setAttachedLinks";
+  case 947: return "FixedJoint_setRestTransform";
+  case 948: return "FixedJoint_getFirstAttachedLink";
+  case 949: return "FixedJoint_getSecondAttachedLink";
+  case 950: return "FixedJoint_getRestTransform";
+  case 951: return "FixedJoint_getTransform";
+  case 952: return "FixedJoint_getTransformDerivative";
+  case 953: return "FixedJoint_getMotionSubspaceVector";
+  case 954: return "FixedJoint_computeChildPosVelAcc";
+  case 955: return "FixedJoint_computeChildVelAcc";
+  case 956: return "FixedJoint_computeChildVel";
+  case 957: return "FixedJoint_computeChildAcc";
+  case 958: return "FixedJoint_computeChildBiasAcc";
+  case 959: return "FixedJoint_computeJointTorque";
+  case 960: return "FixedJoint_setIndex";
+  case 961: return "FixedJoint_getIndex";
+  case 962: return "FixedJoint_setPosCoordsOffset";
+  case 963: return "FixedJoint_getPosCoordsOffset";
+  case 964: return "FixedJoint_setDOFsOffset";
+  case 965: return "FixedJoint_getDOFsOffset";
+  case 966: return "FixedJoint_hasPosLimits";
+  case 967: return "FixedJoint_enablePosLimits";
+  case 968: return "FixedJoint_getPosLimits";
+  case 969: return "FixedJoint_getMinPosLimit";
+  case 970: return "FixedJoint_getMaxPosLimit";
+  case 971: return "FixedJoint_setPosLimits";
+  case 972: return "delete_MovableJointImpl1";
+  case 973: return "MovableJointImpl1_getNrOfPosCoords";
+  case 974: return "MovableJointImpl1_getNrOfDOFs";
+  case 975: return "MovableJointImpl1_setIndex";
+  case 976: return "MovableJointImpl1_getIndex";
+  case 977: return "MovableJointImpl1_setPosCoordsOffset";
+  case 978: return "MovableJointImpl1_getPosCoordsOffset";
+  case 979: return "MovableJointImpl1_setDOFsOffset";
+  case 980: return "MovableJointImpl1_getDOFsOffset";
+  case 981: return "delete_MovableJointImpl2";
+  case 982: return "MovableJointImpl2_getNrOfPosCoords";
+  case 983: return "MovableJointImpl2_getNrOfDOFs";
+  case 984: return "MovableJointImpl2_setIndex";
+  case 985: return "MovableJointImpl2_getIndex";
+  case 986: return "MovableJointImpl2_setPosCoordsOffset";
+  case 987: return "MovableJointImpl2_getPosCoordsOffset";
+  case 988: return "MovableJointImpl2_setDOFsOffset";
+  case 989: return "MovableJointImpl2_getDOFsOffset";
+  case 990: return "delete_MovableJointImpl3";
+  case 991: return "MovableJointImpl3_getNrOfPosCoords";
+  case 992: return "MovableJointImpl3_getNrOfDOFs";
+  case 993: return "MovableJointImpl3_setIndex";
+  case 994: return "MovableJointImpl3_getIndex";
+  case 995: return "MovableJointImpl3_setPosCoordsOffset";
+  case 996: return "MovableJointImpl3_getPosCoordsOffset";
+  case 997: return "MovableJointImpl3_setDOFsOffset";
+  case 998: return "MovableJointImpl3_getDOFsOffset";
+  case 999: return "delete_MovableJointImpl4";
+  case 1000: return "MovableJointImpl4_getNrOfPosCoords";
+  case 1001: return "MovableJointImpl4_getNrOfDOFs";
+  case 1002: return "MovableJointImpl4_setIndex";
+  case 1003: return "MovableJointImpl4_getIndex";
+  case 1004: return "MovableJointImpl4_setPosCoordsOffset";
+  case 1005: return "MovableJointImpl4_getPosCoordsOffset";
+  case 1006: return "MovableJointImpl4_setDOFsOffset";
+  case 1007: return "MovableJointImpl4_getDOFsOffset";
+  case 1008: return "delete_MovableJointImpl5";
+  case 1009: return "MovableJointImpl5_getNrOfPosCoords";
+  case 1010: return "MovableJointImpl5_getNrOfDOFs";
+  case 1011: return "MovableJointImpl5_setIndex";
+  case 1012: return "MovableJointImpl5_getIndex";
+  case 1013: return "MovableJointImpl5_setPosCoordsOffset";
+  case 1014: return "MovableJointImpl5_getPosCoordsOffset";
+  case 1015: return "MovableJointImpl5_setDOFsOffset";
+  case 1016: return "MovableJointImpl5_getDOFsOffset";
+  case 1017: return "delete_MovableJointImpl6";
+  case 1018: return "MovableJointImpl6_getNrOfPosCoords";
+  case 1019: return "MovableJointImpl6_getNrOfDOFs";
+  case 1020: return "MovableJointImpl6_setIndex";
+  case 1021: return "MovableJointImpl6_getIndex";
+  case 1022: return "MovableJointImpl6_setPosCoordsOffset";
+  case 1023: return "MovableJointImpl6_getPosCoordsOffset";
+  case 1024: return "MovableJointImpl6_setDOFsOffset";
+  case 1025: return "MovableJointImpl6_getDOFsOffset";
+  case 1026: return "new_RevoluteJoint";
+  case 1027: return "delete_RevoluteJoint";
+  case 1028: return "RevoluteJoint_clone";
+  case 1029: return "RevoluteJoint_setAttachedLinks";
+  case 1030: return "RevoluteJoint_setRestTransform";
+  case 1031: return "RevoluteJoint_setAxis";
+  case 1032: return "RevoluteJoint_getFirstAttachedLink";
+  case 1033: return "RevoluteJoint_getSecondAttachedLink";
+  case 1034: return "RevoluteJoint_getAxis";
+  case 1035: return "RevoluteJoint_getRestTransform";
+  case 1036: return "RevoluteJoint_getTransform";
+  case 1037: return "RevoluteJoint_getTransformDerivative";
+  case 1038: return "RevoluteJoint_getMotionSubspaceVector";
+  case 1039: return "RevoluteJoint_computeChildPosVelAcc";
+  case 1040: return "RevoluteJoint_computeChildVel";
+  case 1041: return "RevoluteJoint_computeChildVelAcc";
+  case 1042: return "RevoluteJoint_computeChildAcc";
+  case 1043: return "RevoluteJoint_computeChildBiasAcc";
+  case 1044: return "RevoluteJoint_computeJointTorque";
+  case 1045: return "RevoluteJoint_hasPosLimits";
+  case 1046: return "RevoluteJoint_enablePosLimits";
+  case 1047: return "RevoluteJoint_getPosLimits";
+  case 1048: return "RevoluteJoint_getMinPosLimit";
+  case 1049: return "RevoluteJoint_getMaxPosLimit";
+  case 1050: return "RevoluteJoint_setPosLimits";
+  case 1051: return "new_PrismaticJoint";
+  case 1052: return "delete_PrismaticJoint";
+  case 1053: return "PrismaticJoint_clone";
+  case 1054: return "PrismaticJoint_setAttachedLinks";
+  case 1055: return "PrismaticJoint_setRestTransform";
+  case 1056: return "PrismaticJoint_setAxis";
+  case 1057: return "PrismaticJoint_getFirstAttachedLink";
+  case 1058: return "PrismaticJoint_getSecondAttachedLink";
+  case 1059: return "PrismaticJoint_getAxis";
+  case 1060: return "PrismaticJoint_getRestTransform";
+  case 1061: return "PrismaticJoint_getTransform";
+  case 1062: return "PrismaticJoint_getTransformDerivative";
+  case 1063: return "PrismaticJoint_getMotionSubspaceVector";
+  case 1064: return "PrismaticJoint_computeChildPosVelAcc";
+  case 1065: return "PrismaticJoint_computeChildVel";
+  case 1066: return "PrismaticJoint_computeChildVelAcc";
+  case 1067: return "PrismaticJoint_computeChildAcc";
+  case 1068: return "PrismaticJoint_computeChildBiasAcc";
+  case 1069: return "PrismaticJoint_computeJointTorque";
+  case 1070: return "PrismaticJoint_hasPosLimits";
+  case 1071: return "PrismaticJoint_enablePosLimits";
+  case 1072: return "PrismaticJoint_getPosLimits";
+  case 1073: return "PrismaticJoint_getMinPosLimit";
+  case 1074: return "PrismaticJoint_getMaxPosLimit";
+  case 1075: return "PrismaticJoint_setPosLimits";
+  case 1076: return "new_Traversal";
+  case 1077: return "delete_Traversal";
+  case 1078: return "Traversal_getNrOfVisitedLinks";
+  case 1079: return "Traversal_getLink";
+  case 1080: return "Traversal_getBaseLink";
+  case 1081: return "Traversal_getParentLink";
+  case 1082: return "Traversal_getParentJoint";
+  case 1083: return "Traversal_getParentLinkFromLinkIndex";
+  case 1084: return "Traversal_getParentJointFromLinkIndex";
+  case 1085: return "Traversal_getTraversalIndexFromLinkIndex";
+  case 1086: return "Traversal_reset";
+  case 1087: return "Traversal_addTraversalBase";
+  case 1088: return "Traversal_addTraversalElement";
+  case 1089: return "Traversal_isParentOf";
+  case 1090: return "Traversal_getChildLinkIndexFromJointIndex";
+  case 1091: return "Traversal_getParentLinkIndexFromJointIndex";
+  case 1092: return "Traversal_toString";
+  case 1093: return "delete_SolidShape";
+  case 1094: return "SolidShape_clone";
+  case 1095: return "SolidShape_name_get";
+  case 1096: return "SolidShape_name_set";
+  case 1097: return "SolidShape_nameIsValid_get";
+  case 1098: return "SolidShape_nameIsValid_set";
+  case 1099: return "SolidShape_link_H_geometry_get";
+  case 1100: return "SolidShape_link_H_geometry_set";
+  case 1101: return "SolidShape_material_get";
+  case 1102: return "SolidShape_material_set";
+  case 1103: return "SolidShape_isSphere";
+  case 1104: return "SolidShape_isBox";
+  case 1105: return "SolidShape_isCylinder";
+  case 1106: return "SolidShape_isExternalMesh";
+  case 1107: return "SolidShape_asSphere";
+  case 1108: return "SolidShape_asBox";
+  case 1109: return "SolidShape_asCylinder";
+  case 1110: return "SolidShape_asExternalMesh";
+  case 1111: return "delete_Sphere";
+  case 1112: return "Sphere_clone";
+  case 1113: return "Sphere_radius_get";
+  case 1114: return "Sphere_radius_set";
+  case 1115: return "new_Sphere";
+  case 1116: return "delete_Box";
+  case 1117: return "Box_clone";
+  case 1118: return "Box_x_get";
+  case 1119: return "Box_x_set";
+  case 1120: return "Box_y_get";
+  case 1121: return "Box_y_set";
+  case 1122: return "Box_z_get";
+  case 1123: return "Box_z_set";
+  case 1124: return "new_Box";
+  case 1125: return "delete_Cylinder";
+  case 1126: return "Cylinder_clone";
+  case 1127: return "Cylinder_length_get";
+  case 1128: return "Cylinder_length_set";
+  case 1129: return "Cylinder_radius_get";
+  case 1130: return "Cylinder_radius_set";
+  case 1131: return "new_Cylinder";
+  case 1132: return "delete_ExternalMesh";
+  case 1133: return "ExternalMesh_clone";
+  case 1134: return "ExternalMesh_filename_get";
+  case 1135: return "ExternalMesh_filename_set";
+  case 1136: return "ExternalMesh_scale_get";
+  case 1137: return "ExternalMesh_scale_set";
+  case 1138: return "new_ExternalMesh";
+  case 1139: return "new_ModelSolidShapes";
+  case 1140: return "ModelSolidShapes_clear";
+  case 1141: return "delete_ModelSolidShapes";
+  case 1142: return "ModelSolidShapes_resize";
+  case 1143: return "ModelSolidShapes_isConsistent";
+  case 1144: return "ModelSolidShapes_linkSolidShapes_get";
+  case 1145: return "ModelSolidShapes_linkSolidShapes_set";
+  case 1146: return "Neighbor_neighborLink_get";
+  case 1147: return "Neighbor_neighborLink_set";
+  case 1148: return "Neighbor_neighborJoint_get";
+  case 1149: return "Neighbor_neighborJoint_set";
+  case 1150: return "new_Neighbor";
+  case 1151: return "delete_Neighbor";
+  case 1152: return "new_Model";
+  case 1153: return "Model_copy";
+  case 1154: return "delete_Model";
+  case 1155: return "Model_getNrOfLinks";
+  case 1156: return "Model_getLinkName";
+  case 1157: return "Model_getLinkIndex";
+  case 1158: return "Model_isValidLinkIndex";
+  case 1159: return "Model_getLink";
+  case 1160: return "Model_addLink";
+  case 1161: return "Model_getNrOfJoints";
+  case 1162: return "Model_getJointName";
+  case 1163: return "Model_getTotalMass";
+  case 1164: return "Model_getJointIndex";
+  case 1165: return "Model_getJoint";
+  case 1166: return "Model_isValidJointIndex";
+  case 1167: return "Model_isLinkNameUsed";
+  case 1168: return "Model_isJointNameUsed";
+  case 1169: return "Model_isFrameNameUsed";
+  case 1170: return "Model_addJoint";
+  case 1171: return "Model_addJointAndLink";
+  case 1172: return "Model_insertLinkToExistingJointAndAddJointForDisplacedLink";
+  case 1173: return "Model_getNrOfPosCoords";
+  case 1174: return "Model_getNrOfDOFs";
+  case 1175: return "Model_getNrOfFrames";
+  case 1176: return "Model_addAdditionalFrameToLink";
+  case 1177: return "Model_getFrameName";
+  case 1178: return "Model_getFrameIndex";
+  case 1179: return "Model_isValidFrameIndex";
+  case 1180: return "Model_getFrameTransform";
+  case 1181: return "Model_getFrameLink";
+  case 1182: return "Model_getLinkAdditionalFrames";
+  case 1183: return "Model_getNrOfNeighbors";
+  case 1184: return "Model_getNeighbor";
+  case 1185: return "Model_setDefaultBaseLink";
+  case 1186: return "Model_getDefaultBaseLink";
+  case 1187: return "Model_computeFullTreeTraversal";
+  case 1188: return "Model_getInertialParameters";
+  case 1189: return "Model_updateInertialParameters";
+  case 1190: return "Model_visualSolidShapes";
+  case 1191: return "Model_collisionSolidShapes";
+  case 1192: return "Model_toString";
+  case 1193: return "new_JointPosDoubleArray";
+  case 1194: return "JointPosDoubleArray_resize";
+  case 1195: return "JointPosDoubleArray_isConsistent";
+  case 1196: return "delete_JointPosDoubleArray";
+  case 1197: return "new_JointDOFsDoubleArray";
+  case 1198: return "JointDOFsDoubleArray_resize";
+  case 1199: return "JointDOFsDoubleArray_isConsistent";
+  case 1200: return "delete_JointDOFsDoubleArray";
+  case 1201: return "new_DOFSpatialForceArray";
+  case 1202: return "DOFSpatialForceArray_resize";
+  case 1203: return "DOFSpatialForceArray_isConsistent";
+  case 1204: return "DOFSpatialForceArray_paren";
+  case 1205: return "delete_DOFSpatialForceArray";
+  case 1206: return "new_DOFSpatialMotionArray";
+  case 1207: return "DOFSpatialMotionArray_resize";
+  case 1208: return "DOFSpatialMotionArray_isConsistent";
+  case 1209: return "DOFSpatialMotionArray_paren";
+  case 1210: return "delete_DOFSpatialMotionArray";
+  case 1211: return "new_FrameFreeFloatingJacobian";
+  case 1212: return "FrameFreeFloatingJacobian_resize";
+  case 1213: return "FrameFreeFloatingJacobian_isConsistent";
+  case 1214: return "delete_FrameFreeFloatingJacobian";
+  case 1215: return "new_MomentumFreeFloatingJacobian";
+  case 1216: return "MomentumFreeFloatingJacobian_resize";
+  case 1217: return "MomentumFreeFloatingJacobian_isConsistent";
+  case 1218: return "delete_MomentumFreeFloatingJacobian";
+  case 1219: return "new_FreeFloatingMassMatrix";
+  case 1220: return "FreeFloatingMassMatrix_resize";
+  case 1221: return "delete_FreeFloatingMassMatrix";
+  case 1222: return "new_FreeFloatingPos";
+  case 1223: return "FreeFloatingPos_resize";
+  case 1224: return "FreeFloatingPos_worldBasePos";
+  case 1225: return "FreeFloatingPos_jointPos";
+  case 1226: return "FreeFloatingPos_getNrOfPosCoords";
+  case 1227: return "delete_FreeFloatingPos";
+  case 1228: return "new_FreeFloatingGeneralizedTorques";
+  case 1229: return "FreeFloatingGeneralizedTorques_resize";
+  case 1230: return "FreeFloatingGeneralizedTorques_baseWrench";
+  case 1231: return "FreeFloatingGeneralizedTorques_jointTorques";
+  case 1232: return "FreeFloatingGeneralizedTorques_getNrOfDOFs";
+  case 1233: return "delete_FreeFloatingGeneralizedTorques";
+  case 1234: return "new_FreeFloatingVel";
+  case 1235: return "FreeFloatingVel_resize";
+  case 1236: return "FreeFloatingVel_baseVel";
+  case 1237: return "FreeFloatingVel_jointVel";
+  case 1238: return "FreeFloatingVel_getNrOfDOFs";
+  case 1239: return "delete_FreeFloatingVel";
+  case 1240: return "new_FreeFloatingAcc";
+  case 1241: return "FreeFloatingAcc_resize";
+  case 1242: return "FreeFloatingAcc_baseAcc";
+  case 1243: return "FreeFloatingAcc_jointAcc";
+  case 1244: return "FreeFloatingAcc_getNrOfDOFs";
+  case 1245: return "delete_FreeFloatingAcc";
+  case 1246: return "ContactWrench_contactId";
+  case 1247: return "ContactWrench_contactPoint";
+  case 1248: return "ContactWrench_contactWrench";
+  case 1249: return "new_ContactWrench";
+  case 1250: return "delete_ContactWrench";
+  case 1251: return "new_LinkContactWrenches";
+  case 1252: return "LinkContactWrenches_resize";
+  case 1253: return "LinkContactWrenches_getNrOfContactsForLink";
+  case 1254: return "LinkContactWrenches_setNrOfContactsForLink";
+  case 1255: return "LinkContactWrenches_getNrOfLinks";
+  case 1256: return "LinkContactWrenches_contactWrench";
+  case 1257: return "LinkContactWrenches_computeNetWrenches";
+  case 1258: return "LinkContactWrenches_toString";
+  case 1259: return "delete_LinkContactWrenches";
+  case 1260: return "_wrap_ForwardPositionKinematics";
+  case 1261: return "_wrap_ForwardVelAccKinematics";
+  case 1262: return "_wrap_ForwardPosVelAccKinematics";
+  case 1263: return "_wrap_ForwardPosVelKinematics";
+  case 1264: return "_wrap_ForwardAccKinematics";
+  case 1265: return "_wrap_ForwardBiasAccKinematics";
+  case 1266: return "_wrap_ComputeLinearAndAngularMomentum";
+  case 1267: return "_wrap_ComputeLinearAndAngularMomentumDerivativeBias";
+  case 1268: return "_wrap_RNEADynamicPhase";
+  case 1269: return "_wrap_CompositeRigidBodyAlgorithm";
+  case 1270: return "new_ArticulatedBodyAlgorithmInternalBuffers";
+  case 1271: return "ArticulatedBodyAlgorithmInternalBuffers_resize";
+  case 1272: return "ArticulatedBodyAlgorithmInternalBuffers_isConsistent";
+  case 1273: return "ArticulatedBodyAlgorithmInternalBuffers_S_get";
+  case 1274: return "ArticulatedBodyAlgorithmInternalBuffers_S_set";
+  case 1275: return "ArticulatedBodyAlgorithmInternalBuffers_U_get";
+  case 1276: return "ArticulatedBodyAlgorithmInternalBuffers_U_set";
+  case 1277: return "ArticulatedBodyAlgorithmInternalBuffers_D_get";
+  case 1278: return "ArticulatedBodyAlgorithmInternalBuffers_D_set";
+  case 1279: return "ArticulatedBodyAlgorithmInternalBuffers_u_get";
+  case 1280: return "ArticulatedBodyAlgorithmInternalBuffers_u_set";
+  case 1281: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_get";
+  case 1282: return "ArticulatedBodyAlgorithmInternalBuffers_linksVel_set";
+  case 1283: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get";
+  case 1284: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set";
+  case 1285: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get";
+  case 1286: return "ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set";
+  case 1287: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get";
+  case 1288: return "ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set";
+  case 1289: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get";
+  case 1290: return "ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set";
+  case 1291: return "delete_ArticulatedBodyAlgorithmInternalBuffers";
+  case 1292: return "_wrap_ArticulatedBodyAlgorithm";
+  case 1293: return "_wrap_InverseDynamicsInertialParametersRegressor";
+  case 1294: return "NR_OF_SENSOR_TYPES_get";
+  case 1295: return "_wrap_isLinkSensor";
+  case 1296: return "_wrap_isJointSensor";
+  case 1297: return "_wrap_getSensorTypeSize";
+  case 1298: return "delete_Sensor";
+  case 1299: return "Sensor_getName";
+  case 1300: return "Sensor_getSensorType";
+  case 1301: return "Sensor_isValid";
+  case 1302: return "Sensor_setName";
+  case 1303: return "Sensor_clone";
+  case 1304: return "Sensor_isConsistent";
+  case 1305: return "Sensor_updateIndices";
+  case 1306: return "Sensor_updateIndeces";
+  case 1307: return "delete_JointSensor";
+  case 1308: return "JointSensor_getParentJoint";
+  case 1309: return "JointSensor_getParentJointIndex";
+  case 1310: return "JointSensor_setParentJoint";
+  case 1311: return "JointSensor_setParentJointIndex";
+  case 1312: return "JointSensor_isConsistent";
+  case 1313: return "delete_LinkSensor";
+  case 1314: return "LinkSensor_getParentLink";
+  case 1315: return "LinkSensor_getParentLinkIndex";
+  case 1316: return "LinkSensor_getLinkSensorTransform";
+  case 1317: return "LinkSensor_setParentLink";
+  case 1318: return "LinkSensor_setParentLinkIndex";
+  case 1319: return "LinkSensor_setLinkSensorTransform";
+  case 1320: return "LinkSensor_isConsistent";
+  case 1321: return "new_SensorsList";
+  case 1322: return "delete_SensorsList";
+  case 1323: return "SensorsList_addSensor";
+  case 1324: return "SensorsList_setSerialization";
+  case 1325: return "SensorsList_getSerialization";
+  case 1326: return "SensorsList_getNrOfSensors";
+  case 1327: return "SensorsList_getSensorIndex";
+  case 1328: return "SensorsList_getSizeOfAllSensorsMeasurements";
+  case 1329: return "SensorsList_getSensor";
+  case 1330: return "SensorsList_isConsistent";
+  case 1331: return "SensorsList_removeSensor";
+  case 1332: return "SensorsList_removeAllSensorsOfType";
+  case 1333: return "SensorsList_getSixAxisForceTorqueSensor";
+  case 1334: return "SensorsList_getAccelerometerSensor";
+  case 1335: return "SensorsList_getGyroscopeSensor";
+  case 1336: return "SensorsList_getThreeAxisAngularAccelerometerSensor";
+  case 1337: return "SensorsList_getThreeAxisForceTorqueContactSensor";
+  case 1338: return "new_SensorsMeasurements";
+  case 1339: return "delete_SensorsMeasurements";
+  case 1340: return "SensorsMeasurements_setNrOfSensors";
+  case 1341: return "SensorsMeasurements_getNrOfSensors";
+  case 1342: return "SensorsMeasurements_resize";
+  case 1343: return "SensorsMeasurements_toVector";
+  case 1344: return "SensorsMeasurements_setMeasurement";
+  case 1345: return "SensorsMeasurements_getMeasurement";
+  case 1346: return "SensorsMeasurements_getSizeOfAllSensorsMeasurements";
+  case 1347: return "new_SixAxisForceTorqueSensor";
+  case 1348: return "delete_SixAxisForceTorqueSensor";
+  case 1349: return "SixAxisForceTorqueSensor_setName";
+  case 1350: return "SixAxisForceTorqueSensor_setFirstLinkSensorTransform";
+  case 1351: return "SixAxisForceTorqueSensor_setSecondLinkSensorTransform";
+  case 1352: return "SixAxisForceTorqueSensor_getFirstLinkIndex";
+  case 1353: return "SixAxisForceTorqueSensor_getSecondLinkIndex";
+  case 1354: return "SixAxisForceTorqueSensor_setFirstLinkName";
+  case 1355: return "SixAxisForceTorqueSensor_setSecondLinkName";
+  case 1356: return "SixAxisForceTorqueSensor_getFirstLinkName";
+  case 1357: return "SixAxisForceTorqueSensor_getSecondLinkName";
+  case 1358: return "SixAxisForceTorqueSensor_setParentJoint";
+  case 1359: return "SixAxisForceTorqueSensor_setParentJointIndex";
+  case 1360: return "SixAxisForceTorqueSensor_setAppliedWrenchLink";
+  case 1361: return "SixAxisForceTorqueSensor_getName";
+  case 1362: return "SixAxisForceTorqueSensor_getSensorType";
+  case 1363: return "SixAxisForceTorqueSensor_getParentJoint";
+  case 1364: return "SixAxisForceTorqueSensor_getParentJointIndex";
+  case 1365: return "SixAxisForceTorqueSensor_isValid";
+  case 1366: return "SixAxisForceTorqueSensor_clone";
+  case 1367: return "SixAxisForceTorqueSensor_updateIndices";
+  case 1368: return "SixAxisForceTorqueSensor_updateIndeces";
+  case 1369: return "SixAxisForceTorqueSensor_getAppliedWrenchLink";
+  case 1370: return "SixAxisForceTorqueSensor_isLinkAttachedToSensor";
+  case 1371: return "SixAxisForceTorqueSensor_getLinkSensorTransform";
+  case 1372: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLink";
+  case 1373: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix";
+  case 1374: return "SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix";
+  case 1375: return "SixAxisForceTorqueSensor_predictMeasurement";
+  case 1376: return "SixAxisForceTorqueSensor_toString";
+  case 1377: return "new_AccelerometerSensor";
+  case 1378: return "delete_AccelerometerSensor";
+  case 1379: return "AccelerometerSensor_setName";
+  case 1380: return "AccelerometerSensor_setLinkSensorTransform";
+  case 1381: return "AccelerometerSensor_setParentLink";
+  case 1382: return "AccelerometerSensor_setParentLinkIndex";
+  case 1383: return "AccelerometerSensor_getName";
+  case 1384: return "AccelerometerSensor_getSensorType";
+  case 1385: return "AccelerometerSensor_getParentLink";
+  case 1386: return "AccelerometerSensor_getParentLinkIndex";
+  case 1387: return "AccelerometerSensor_getLinkSensorTransform";
+  case 1388: return "AccelerometerSensor_isValid";
+  case 1389: return "AccelerometerSensor_clone";
+  case 1390: return "AccelerometerSensor_updateIndices";
+  case 1391: return "AccelerometerSensor_updateIndeces";
+  case 1392: return "AccelerometerSensor_predictMeasurement";
+  case 1393: return "new_GyroscopeSensor";
+  case 1394: return "delete_GyroscopeSensor";
+  case 1395: return "GyroscopeSensor_setName";
+  case 1396: return "GyroscopeSensor_setLinkSensorTransform";
+  case 1397: return "GyroscopeSensor_setParentLink";
+  case 1398: return "GyroscopeSensor_setParentLinkIndex";
+  case 1399: return "GyroscopeSensor_getName";
+  case 1400: return "GyroscopeSensor_getSensorType";
+  case 1401: return "GyroscopeSensor_getParentLink";
+  case 1402: return "GyroscopeSensor_getParentLinkIndex";
+  case 1403: return "GyroscopeSensor_getLinkSensorTransform";
+  case 1404: return "GyroscopeSensor_isValid";
+  case 1405: return "GyroscopeSensor_clone";
+  case 1406: return "GyroscopeSensor_updateIndices";
+  case 1407: return "GyroscopeSensor_updateIndeces";
+  case 1408: return "GyroscopeSensor_predictMeasurement";
+  case 1409: return "new_ThreeAxisAngularAccelerometerSensor";
+  case 1410: return "delete_ThreeAxisAngularAccelerometerSensor";
+  case 1411: return "ThreeAxisAngularAccelerometerSensor_setName";
+  case 1412: return "ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform";
+  case 1413: return "ThreeAxisAngularAccelerometerSensor_setParentLink";
+  case 1414: return "ThreeAxisAngularAccelerometerSensor_setParentLinkIndex";
+  case 1415: return "ThreeAxisAngularAccelerometerSensor_getName";
+  case 1416: return "ThreeAxisAngularAccelerometerSensor_getSensorType";
+  case 1417: return "ThreeAxisAngularAccelerometerSensor_getParentLink";
+  case 1418: return "ThreeAxisAngularAccelerometerSensor_getParentLinkIndex";
+  case 1419: return "ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform";
+  case 1420: return "ThreeAxisAngularAccelerometerSensor_isValid";
+  case 1421: return "ThreeAxisAngularAccelerometerSensor_clone";
+  case 1422: return "ThreeAxisAngularAccelerometerSensor_updateIndices";
+  case 1423: return "ThreeAxisAngularAccelerometerSensor_predictMeasurement";
+  case 1424: return "new_ThreeAxisForceTorqueContactSensor";
+  case 1425: return "delete_ThreeAxisForceTorqueContactSensor";
+  case 1426: return "ThreeAxisForceTorqueContactSensor_setName";
+  case 1427: return "ThreeAxisForceTorqueContactSensor_setLinkSensorTransform";
+  case 1428: return "ThreeAxisForceTorqueContactSensor_setParentLink";
+  case 1429: return "ThreeAxisForceTorqueContactSensor_setParentLinkIndex";
+  case 1430: return "ThreeAxisForceTorqueContactSensor_getName";
+  case 1431: return "ThreeAxisForceTorqueContactSensor_getSensorType";
+  case 1432: return "ThreeAxisForceTorqueContactSensor_getParentLink";
+  case 1433: return "ThreeAxisForceTorqueContactSensor_getParentLinkIndex";
+  case 1434: return "ThreeAxisForceTorqueContactSensor_getLinkSensorTransform";
+  case 1435: return "ThreeAxisForceTorqueContactSensor_isValid";
+  case 1436: return "ThreeAxisForceTorqueContactSensor_clone";
+  case 1437: return "ThreeAxisForceTorqueContactSensor_updateIndices";
+  case 1438: return "ThreeAxisForceTorqueContactSensor_setLoadCellLocations";
+  case 1439: return "ThreeAxisForceTorqueContactSensor_getLoadCellLocations";
+  case 1440: return "ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements";
+  case 1441: return "ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements";
+  case 1442: return "_wrap_predictSensorsMeasurements";
+  case 1443: return "_wrap_predictSensorsMeasurementsFromRawBuffers";
+  case 1444: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_get";
+  case 1445: return "URDFParserOptions_addSensorFramesAsAdditionalFrames_set";
+  case 1446: return "URDFParserOptions_originalFilename_get";
+  case 1447: return "URDFParserOptions_originalFilename_set";
+  case 1448: return "new_URDFParserOptions";
+  case 1449: return "delete_URDFParserOptions";
+  case 1450: return "_wrap_modelFromURDF";
+  case 1451: return "_wrap_modelFromURDFString";
+  case 1452: return "_wrap_dofsListFromURDF";
+  case 1453: return "_wrap_dofsListFromURDFString";
+  case 1454: return "_wrap_sensorsFromURDF";
+  case 1455: return "_wrap_sensorsFromURDFString";
+  case 1456: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_get";
+  case 1457: return "ModelParserOptions_addSensorFramesAsAdditionalFrames_set";
+  case 1458: return "ModelParserOptions_originalFilename_get";
+  case 1459: return "ModelParserOptions_originalFilename_set";
+  case 1460: return "new_ModelParserOptions";
+  case 1461: return "delete_ModelParserOptions";
+  case 1462: return "new_ModelLoader";
+  case 1463: return "delete_ModelLoader";
+  case 1464: return "ModelLoader_parsingOptions";
+  case 1465: return "ModelLoader_setParsingOptions";
+  case 1466: return "ModelLoader_loadModelFromString";
+  case 1467: return "ModelLoader_loadModelFromFile";
+  case 1468: return "ModelLoader_loadReducedModelFromFullModel";
+  case 1469: return "ModelLoader_loadReducedModelFromString";
+  case 1470: return "ModelLoader_loadReducedModelFromFile";
+  case 1471: return "ModelLoader_model";
+  case 1472: return "ModelLoader_sensors";
+  case 1473: return "ModelLoader_isValid";
+  case 1474: return "ModelExporterOptions_baseLink_get";
+  case 1475: return "ModelExporterOptions_baseLink_set";
+  case 1476: return "ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_get";
+  case 1477: return "ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_set";
+  case 1478: return "new_ModelExporterOptions";
+  case 1479: return "delete_ModelExporterOptions";
+  case 1480: return "new_ModelExporter";
+  case 1481: return "delete_ModelExporter";
+  case 1482: return "ModelExporter_exportingOptions";
+  case 1483: return "ModelExporter_setExportingOptions";
+  case 1484: return "ModelExporter_init";
+  case 1485: return "ModelExporter_model";
+  case 1486: return "ModelExporter_sensors";
+  case 1487: return "ModelExporter_isValid";
+  case 1488: return "ModelExporter_exportModelToString";
+  case 1489: return "ModelExporter_exportModelToFile";
+  case 1490: return "new_UnknownWrenchContact";
+  case 1491: return "UnknownWrenchContact_unknownType_get";
+  case 1492: return "UnknownWrenchContact_unknownType_set";
+  case 1493: return "UnknownWrenchContact_contactPoint_get";
+  case 1494: return "UnknownWrenchContact_contactPoint_set";
+  case 1495: return "UnknownWrenchContact_forceDirection_get";
+  case 1496: return "UnknownWrenchContact_forceDirection_set";
+  case 1497: return "UnknownWrenchContact_knownWrench_get";
+  case 1498: return "UnknownWrenchContact_knownWrench_set";
+  case 1499: return "UnknownWrenchContact_contactId_get";
+  case 1500: return "UnknownWrenchContact_contactId_set";
+  case 1501: return "delete_UnknownWrenchContact";
+  case 1502: return "new_LinkUnknownWrenchContacts";
+  case 1503: return "LinkUnknownWrenchContacts_clear";
+  case 1504: return "LinkUnknownWrenchContacts_resize";
+  case 1505: return "LinkUnknownWrenchContacts_getNrOfContactsForLink";
+  case 1506: return "LinkUnknownWrenchContacts_setNrOfContactsForLink";
+  case 1507: return "LinkUnknownWrenchContacts_addNewContactForLink";
+  case 1508: return "LinkUnknownWrenchContacts_addNewContactInFrame";
+  case 1509: return "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin";
+  case 1510: return "LinkUnknownWrenchContacts_contactWrench";
+  case 1511: return "LinkUnknownWrenchContacts_toString";
+  case 1512: return "delete_LinkUnknownWrenchContacts";
+  case 1513: return "new_estimateExternalWrenchesBuffers";
+  case 1514: return "estimateExternalWrenchesBuffers_resize";
+  case 1515: return "estimateExternalWrenchesBuffers_getNrOfSubModels";
+  case 1516: return "estimateExternalWrenchesBuffers_getNrOfLinks";
+  case 1517: return "estimateExternalWrenchesBuffers_isConsistent";
+  case 1518: return "estimateExternalWrenchesBuffers_A_get";
+  case 1519: return "estimateExternalWrenchesBuffers_A_set";
+  case 1520: return "estimateExternalWrenchesBuffers_x_get";
+  case 1521: return "estimateExternalWrenchesBuffers_x_set";
+  case 1522: return "estimateExternalWrenchesBuffers_b_get";
+  case 1523: return "estimateExternalWrenchesBuffers_b_set";
+  case 1524: return "estimateExternalWrenchesBuffers_pinvA_get";
+  case 1525: return "estimateExternalWrenchesBuffers_pinvA_set";
+  case 1526: return "estimateExternalWrenchesBuffers_b_contacts_subtree_get";
+  case 1527: return "estimateExternalWrenchesBuffers_b_contacts_subtree_set";
+  case 1528: return "estimateExternalWrenchesBuffers_subModelBase_H_link_get";
+  case 1529: return "estimateExternalWrenchesBuffers_subModelBase_H_link_set";
+  case 1530: return "delete_estimateExternalWrenchesBuffers";
+  case 1531: return "_wrap_estimateExternalWrenchesWithoutInternalFT";
+  case 1532: return "_wrap_estimateExternalWrenches";
+  case 1533: return "_wrap_dynamicsEstimationForwardVelAccKinematics";
+  case 1534: return "_wrap_dynamicsEstimationForwardVelKinematics";
+  case 1535: return "_wrap_computeLinkNetWrenchesWithoutGravity";
+  case 1536: return "_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches";
+  case 1537: return "new_ExtWrenchesAndJointTorquesEstimator";
+  case 1538: return "delete_ExtWrenchesAndJointTorquesEstimator";
+  case 1539: return "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors";
+  case 1540: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile";
+  case 1541: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs";
+  case 1542: return "ExtWrenchesAndJointTorquesEstimator_model";
+  case 1543: return "ExtWrenchesAndJointTorquesEstimator_sensors";
+  case 1544: return "ExtWrenchesAndJointTorquesEstimator_submodels";
+  case 1545: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase";
+  case 1546: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase";
+  case 1547: return "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements";
+  case 1548: return "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques";
+  case 1549: return "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill";
+  case 1550: return "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity";
+  case 1551: return "new_SimpleLeggedOdometry";
+  case 1552: return "delete_SimpleLeggedOdometry";
+  case 1553: return "SimpleLeggedOdometry_setModel";
+  case 1554: return "SimpleLeggedOdometry_loadModelFromFile";
+  case 1555: return "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs";
+  case 1556: return "SimpleLeggedOdometry_model";
+  case 1557: return "SimpleLeggedOdometry_updateKinematics";
+  case 1558: return "SimpleLeggedOdometry_init";
+  case 1559: return "SimpleLeggedOdometry_changeFixedFrame";
+  case 1560: return "SimpleLeggedOdometry_getCurrentFixedLink";
+  case 1561: return "SimpleLeggedOdometry_getWorldLinkTransform";
+  case 1562: return "SimpleLeggedOdometry_getWorldFrameTransform";
+  case 1563: return "_wrap_isLinkBerdyDynamicVariable";
+  case 1564: return "_wrap_isJointBerdyDynamicVariable";
+  case 1565: return "_wrap_isDOFBerdyDynamicVariable";
+  case 1566: return "new_BerdyOptions";
+  case 1567: return "BerdyOptions_berdyVariant_get";
+  case 1568: return "BerdyOptions_berdyVariant_set";
+  case 1569: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get";
+  case 1570: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set";
+  case 1571: return "BerdyOptions_includeAllJointAccelerationsAsSensors_get";
+  case 1572: return "BerdyOptions_includeAllJointAccelerationsAsSensors_set";
+  case 1573: return "BerdyOptions_includeAllJointTorquesAsSensors_get";
+  case 1574: return "BerdyOptions_includeAllJointTorquesAsSensors_set";
+  case 1575: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get";
+  case 1576: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set";
+  case 1577: return "BerdyOptions_includeFixedBaseExternalWrench_get";
+  case 1578: return "BerdyOptions_includeFixedBaseExternalWrench_set";
+  case 1579: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get";
+  case 1580: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set";
+  case 1581: return "BerdyOptions_baseLink_get";
+  case 1582: return "BerdyOptions_baseLink_set";
+  case 1583: return "BerdyOptions_checkConsistency";
+  case 1584: return "delete_BerdyOptions";
+  case 1585: return "BerdySensor_type_get";
+  case 1586: return "BerdySensor_type_set";
+  case 1587: return "BerdySensor_id_get";
+  case 1588: return "BerdySensor_id_set";
+  case 1589: return "BerdySensor_range_get";
+  case 1590: return "BerdySensor_range_set";
+  case 1591: return "BerdySensor_eq";
+  case 1592: return "BerdySensor_lt";
+  case 1593: return "new_BerdySensor";
+  case 1594: return "delete_BerdySensor";
+  case 1595: return "BerdyDynamicVariable_type_get";
+  case 1596: return "BerdyDynamicVariable_type_set";
+  case 1597: return "BerdyDynamicVariable_id_get";
+  case 1598: return "BerdyDynamicVariable_id_set";
+  case 1599: return "BerdyDynamicVariable_range_get";
+  case 1600: return "BerdyDynamicVariable_range_set";
+  case 1601: return "BerdyDynamicVariable_eq";
+  case 1602: return "BerdyDynamicVariable_lt";
+  case 1603: return "new_BerdyDynamicVariable";
+  case 1604: return "delete_BerdyDynamicVariable";
+  case 1605: return "new_BerdyHelper";
+  case 1606: return "BerdyHelper_dynamicTraversal";
+  case 1607: return "BerdyHelper_model";
+  case 1608: return "BerdyHelper_sensors";
+  case 1609: return "BerdyHelper_isValid";
+  case 1610: return "BerdyHelper_init";
+  case 1611: return "BerdyHelper_getOptions";
+  case 1612: return "BerdyHelper_getNrOfDynamicVariables";
+  case 1613: return "BerdyHelper_getNrOfDynamicEquations";
+  case 1614: return "BerdyHelper_getNrOfSensorsMeasurements";
+  case 1615: return "BerdyHelper_resizeAndZeroBerdyMatrices";
+  case 1616: return "BerdyHelper_getBerdyMatrices";
+  case 1617: return "BerdyHelper_getSensorsOrdering";
+  case 1618: return "BerdyHelper_getRangeSensorVariable";
+  case 1619: return "BerdyHelper_getRangeDOFSensorVariable";
+  case 1620: return "BerdyHelper_getRangeJointSensorVariable";
+  case 1621: return "BerdyHelper_getRangeLinkSensorVariable";
+  case 1622: return "BerdyHelper_getRangeLinkVariable";
+  case 1623: return "BerdyHelper_getRangeJointVariable";
+  case 1624: return "BerdyHelper_getRangeDOFVariable";
+  case 1625: return "BerdyHelper_getDynamicVariablesOrdering";
+  case 1626: return "BerdyHelper_serializeDynamicVariables";
+  case 1627: return "BerdyHelper_serializeSensorVariables";
+  case 1628: return "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA";
+  case 1629: return "BerdyHelper_extractJointTorquesFromDynamicVariables";
+  case 1630: return "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables";
+  case 1631: return "BerdyHelper_updateKinematicsFromFloatingBase";
+  case 1632: return "BerdyHelper_updateKinematicsFromFixedBase";
+  case 1633: return "BerdyHelper_updateKinematicsFromTraversalFixedBase";
+  case 1634: return "BerdyHelper_setNetExternalWrenchMeasurementFrame";
+  case 1635: return "BerdyHelper_getNetExternalWrenchMeasurementFrame";
+  case 1636: return "delete_BerdyHelper";
+  case 1637: return "new_BerdySparseMAPSolver";
+  case 1638: return "delete_BerdySparseMAPSolver";
+  case 1639: return "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance";
+  case 1640: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance";
+  case 1641: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue";
+  case 1642: return "BerdySparseMAPSolver_setMeasurementsPriorCovariance";
+  case 1643: return "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse";
+  case 1644: return "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse";
+  case 1645: return "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue";
+  case 1646: return "BerdySparseMAPSolver_measurementsPriorCovarianceInverse";
+  case 1647: return "BerdySparseMAPSolver_isValid";
+  case 1648: return "BerdySparseMAPSolver_initialize";
+  case 1649: return "BerdySparseMAPSolver_updateEstimateInformationFixedBase";
+  case 1650: return "BerdySparseMAPSolver_updateEstimateInformationFloatingBase";
+  case 1651: return "BerdySparseMAPSolver_doEstimate";
+  case 1652: return "BerdySparseMAPSolver_getLastEstimate";
+  case 1653: return "AttitudeEstimatorState_m_orientation_get";
+  case 1654: return "AttitudeEstimatorState_m_orientation_set";
+  case 1655: return "AttitudeEstimatorState_m_angular_velocity_get";
+  case 1656: return "AttitudeEstimatorState_m_angular_velocity_set";
+  case 1657: return "AttitudeEstimatorState_m_gyroscope_bias_get";
+  case 1658: return "AttitudeEstimatorState_m_gyroscope_bias_set";
+  case 1659: return "new_AttitudeEstimatorState";
+  case 1660: return "delete_AttitudeEstimatorState";
+  case 1661: return "delete_IAttitudeEstimator";
+  case 1662: return "IAttitudeEstimator_updateFilterWithMeasurements";
+  case 1663: return "IAttitudeEstimator_propagateStates";
+  case 1664: return "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix";
+  case 1665: return "IAttitudeEstimator_getOrientationEstimateAsQuaternion";
+  case 1666: return "IAttitudeEstimator_getOrientationEstimateAsRPY";
+  case 1667: return "IAttitudeEstimator_getInternalStateSize";
+  case 1668: return "IAttitudeEstimator_getInternalState";
+  case 1669: return "IAttitudeEstimator_getDefaultInternalInitialState";
+  case 1670: return "IAttitudeEstimator_setInternalState";
+  case 1671: return "IAttitudeEstimator_setInternalStateInitialOrientation";
+  case 1672: return "AttitudeMahonyFilterParameters_time_step_in_seconds_get";
+  case 1673: return "AttitudeMahonyFilterParameters_time_step_in_seconds_set";
+  case 1674: return "AttitudeMahonyFilterParameters_kp_get";
+  case 1675: return "AttitudeMahonyFilterParameters_kp_set";
+  case 1676: return "AttitudeMahonyFilterParameters_ki_get";
+  case 1677: return "AttitudeMahonyFilterParameters_ki_set";
+  case 1678: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_get";
+  case 1679: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set";
+  case 1680: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get";
+  case 1681: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set";
+  case 1682: return "new_AttitudeMahonyFilterParameters";
+  case 1683: return "delete_AttitudeMahonyFilterParameters";
+  case 1684: return "new_AttitudeMahonyFilter";
+  case 1685: return "AttitudeMahonyFilter_useMagnetoMeterMeasurements";
+  case 1686: return "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements";
+  case 1687: return "AttitudeMahonyFilter_setGainkp";
+  case 1688: return "AttitudeMahonyFilter_setGainki";
+  case 1689: return "AttitudeMahonyFilter_setTimeStepInSeconds";
+  case 1690: return "AttitudeMahonyFilter_setGravityDirection";
+  case 1691: return "AttitudeMahonyFilter_setParameters";
+  case 1692: return "AttitudeMahonyFilter_getParameters";
+  case 1693: return "AttitudeMahonyFilter_updateFilterWithMeasurements";
+  case 1694: return "AttitudeMahonyFilter_propagateStates";
+  case 1695: return "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix";
+  case 1696: return "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion";
+  case 1697: return "AttitudeMahonyFilter_getOrientationEstimateAsRPY";
+  case 1698: return "AttitudeMahonyFilter_getInternalStateSize";
+  case 1699: return "AttitudeMahonyFilter_getInternalState";
+  case 1700: return "AttitudeMahonyFilter_getDefaultInternalInitialState";
+  case 1701: return "AttitudeMahonyFilter_setInternalState";
+  case 1702: return "AttitudeMahonyFilter_setInternalStateInitialOrientation";
+  case 1703: return "delete_AttitudeMahonyFilter";
+  case 1704: return "DiscreteExtendedKalmanFilterHelper_ekf_f";
+  case 1705: return "DiscreteExtendedKalmanFilterHelper_ekf_h";
+  case 1706: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF";
+  case 1707: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH";
+  case 1708: return "DiscreteExtendedKalmanFilterHelper_ekfPredict";
+  case 1709: return "DiscreteExtendedKalmanFilterHelper_ekfUpdate";
+  case 1710: return "DiscreteExtendedKalmanFilterHelper_ekfInit";
+  case 1711: return "DiscreteExtendedKalmanFilterHelper_ekfReset";
+  case 1712: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector";
+  case 1713: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector";
+  case 1714: return "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState";
+  case 1715: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance";
+  case 1716: return "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance";
+  case 1717: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance";
+  case 1718: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize";
+  case 1719: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize";
+  case 1720: return "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize";
+  case 1721: return "DiscreteExtendedKalmanFilterHelper_ekfGetStates";
+  case 1722: return "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance";
+  case 1723: return "delete_DiscreteExtendedKalmanFilterHelper";
+  case 1724: return "output_dimensions_with_magnetometer_get";
+  case 1725: return "output_dimensions_without_magnetometer_get";
+  case 1726: return "input_dimensions_get";
+  case 1727: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_get";
+  case 1728: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_set";
+  case 1729: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get";
+  case 1730: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set";
+  case 1731: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get";
+  case 1732: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set";
+  case 1733: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get";
+  case 1734: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set";
+  case 1735: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get";
+  case 1736: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set";
+  case 1737: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get";
+  case 1738: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set";
+  case 1739: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get";
+  case 1740: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set";
+  case 1741: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get";
+  case 1742: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set";
+  case 1743: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get";
+  case 1744: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set";
+  case 1745: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get";
+  case 1746: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set";
+  case 1747: return "new_AttitudeQuaternionEKFParameters";
+  case 1748: return "delete_AttitudeQuaternionEKFParameters";
+  case 1749: return "new_AttitudeQuaternionEKF";
+  case 1750: return "AttitudeQuaternionEKF_getParameters";
+  case 1751: return "AttitudeQuaternionEKF_setParameters";
+  case 1752: return "AttitudeQuaternionEKF_setGravityDirection";
+  case 1753: return "AttitudeQuaternionEKF_setTimeStepInSeconds";
+  case 1754: return "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor";
+  case 1755: return "AttitudeQuaternionEKF_useMagnetometerMeasurements";
+  case 1756: return "AttitudeQuaternionEKF_setMeasurementNoiseVariance";
+  case 1757: return "AttitudeQuaternionEKF_setSystemNoiseVariance";
+  case 1758: return "AttitudeQuaternionEKF_setInitialStateCovariance";
+  case 1759: return "AttitudeQuaternionEKF_initializeFilter";
+  case 1760: return "AttitudeQuaternionEKF_updateFilterWithMeasurements";
+  case 1761: return "AttitudeQuaternionEKF_propagateStates";
+  case 1762: return "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix";
+  case 1763: return "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion";
+  case 1764: return "AttitudeQuaternionEKF_getOrientationEstimateAsRPY";
+  case 1765: return "AttitudeQuaternionEKF_getInternalStateSize";
+  case 1766: return "AttitudeQuaternionEKF_getInternalState";
+  case 1767: return "AttitudeQuaternionEKF_getDefaultInternalInitialState";
+  case 1768: return "AttitudeQuaternionEKF_setInternalState";
+  case 1769: return "AttitudeQuaternionEKF_setInternalStateInitialOrientation";
+  case 1770: return "delete_AttitudeQuaternionEKF";
+  case 1771: return "_wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass";
+  case 1772: return "DynamicsRegressorParameter_category_get";
+  case 1773: return "DynamicsRegressorParameter_category_set";
+  case 1774: return "DynamicsRegressorParameter_elemIndex_get";
+  case 1775: return "DynamicsRegressorParameter_elemIndex_set";
+  case 1776: return "DynamicsRegressorParameter_type_get";
+  case 1777: return "DynamicsRegressorParameter_type_set";
+  case 1778: return "DynamicsRegressorParameter_lt";
+  case 1779: return "DynamicsRegressorParameter_eq";
+  case 1780: return "DynamicsRegressorParameter_ne";
+  case 1781: return "new_DynamicsRegressorParameter";
+  case 1782: return "delete_DynamicsRegressorParameter";
+  case 1783: return "DynamicsRegressorParametersList_parameters_get";
+  case 1784: return "DynamicsRegressorParametersList_parameters_set";
+  case 1785: return "DynamicsRegressorParametersList_getDescriptionOfParameter";
+  case 1786: return "DynamicsRegressorParametersList_addParam";
+  case 1787: return "DynamicsRegressorParametersList_addList";
+  case 1788: return "DynamicsRegressorParametersList_findParam";
+  case 1789: return "DynamicsRegressorParametersList_getNrOfParameters";
+  case 1790: return "new_DynamicsRegressorParametersList";
+  case 1791: return "delete_DynamicsRegressorParametersList";
+  case 1792: return "new_DynamicsRegressorGenerator";
+  case 1793: return "delete_DynamicsRegressorGenerator";
+  case 1794: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile";
+  case 1795: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString";
+  case 1796: return "DynamicsRegressorGenerator_loadRegressorStructureFromFile";
+  case 1797: return "DynamicsRegressorGenerator_loadRegressorStructureFromString";
+  case 1798: return "DynamicsRegressorGenerator_isValid";
+  case 1799: return "DynamicsRegressorGenerator_getNrOfParameters";
+  case 1800: return "DynamicsRegressorGenerator_getNrOfOutputs";
+  case 1801: return "DynamicsRegressorGenerator_getNrOfDegreesOfFreedom";
+  case 1802: return "DynamicsRegressorGenerator_getDescriptionOfParameter";
+  case 1803: return "DynamicsRegressorGenerator_getDescriptionOfParameters";
+  case 1804: return "DynamicsRegressorGenerator_getDescriptionOfOutput";
+  case 1805: return "DynamicsRegressorGenerator_getDescriptionOfOutputs";
+  case 1806: return "DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom";
+  case 1807: return "DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom";
+  case 1808: return "DynamicsRegressorGenerator_getDescriptionOfLink";
+  case 1809: return "DynamicsRegressorGenerator_getDescriptionOfLinks";
+  case 1810: return "DynamicsRegressorGenerator_getNrOfLinks";
+  case 1811: return "DynamicsRegressorGenerator_getNrOfFakeLinks";
+  case 1812: return "DynamicsRegressorGenerator_getBaseLinkName";
+  case 1813: return "DynamicsRegressorGenerator_getSensorsModel";
+  case 1814: return "DynamicsRegressorGenerator_setRobotState";
+  case 1815: return "DynamicsRegressorGenerator_getSensorsMeasurements";
+  case 1816: return "DynamicsRegressorGenerator_setTorqueSensorMeasurement";
+  case 1817: return "DynamicsRegressorGenerator_computeRegressor";
+  case 1818: return "DynamicsRegressorGenerator_getModelParameters";
+  case 1819: return "DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace";
+  case 1820: return "DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace";
+  case 1821: return "DynamicsRegressorGenerator_generate_random_regressors";
+  case 1822: return "new_KinDynComputations";
+  case 1823: return "delete_KinDynComputations";
+  case 1824: return "KinDynComputations_loadRobotModel";
+  case 1825: return "KinDynComputations_loadRobotModelFromFile";
+  case 1826: return "KinDynComputations_loadRobotModelFromString";
+  case 1827: return "KinDynComputations_isValid";
+  case 1828: return "KinDynComputations_setFrameVelocityRepresentation";
+  case 1829: return "KinDynComputations_getFrameVelocityRepresentation";
+  case 1830: return "KinDynComputations_getNrOfDegreesOfFreedom";
+  case 1831: return "KinDynComputations_getDescriptionOfDegreeOfFreedom";
+  case 1832: return "KinDynComputations_getDescriptionOfDegreesOfFreedom";
+  case 1833: return "KinDynComputations_getNrOfLinks";
+  case 1834: return "KinDynComputations_getNrOfFrames";
+  case 1835: return "KinDynComputations_getFloatingBase";
+  case 1836: return "KinDynComputations_setFloatingBase";
+  case 1837: return "KinDynComputations_model";
+  case 1838: return "KinDynComputations_getRobotModel";
+  case 1839: return "KinDynComputations_getRelativeJacobianSparsityPattern";
+  case 1840: return "KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern";
+  case 1841: return "KinDynComputations_setJointPos";
+  case 1842: return "KinDynComputations_setRobotState";
+  case 1843: return "KinDynComputations_getRobotState";
+  case 1844: return "KinDynComputations_getWorldBaseTransform";
+  case 1845: return "KinDynComputations_getBaseTwist";
+  case 1846: return "KinDynComputations_getJointPos";
+  case 1847: return "KinDynComputations_getJointVel";
+  case 1848: return "KinDynComputations_getModelVel";
+  case 1849: return "KinDynComputations_getFrameIndex";
+  case 1850: return "KinDynComputations_getFrameName";
+  case 1851: return "KinDynComputations_getWorldTransform";
+  case 1852: return "KinDynComputations_getRelativeTransformExplicit";
+  case 1853: return "KinDynComputations_getRelativeTransform";
+  case 1854: return "KinDynComputations_getFrameVel";
+  case 1855: return "KinDynComputations_getFrameAcc";
+  case 1856: return "KinDynComputations_getFrameFreeFloatingJacobian";
+  case 1857: return "KinDynComputations_getRelativeJacobian";
+  case 1858: return "KinDynComputations_getRelativeJacobianExplicit";
+  case 1859: return "KinDynComputations_getFrameBiasAcc";
+  case 1860: return "KinDynComputations_getCenterOfMassPosition";
+  case 1861: return "KinDynComputations_getCenterOfMassVelocity";
+  case 1862: return "KinDynComputations_getCenterOfMassJacobian";
+  case 1863: return "KinDynComputations_getCenterOfMassBiasAcc";
+  case 1864: return "KinDynComputations_getAverageVelocity";
+  case 1865: return "KinDynComputations_getAverageVelocityJacobian";
+  case 1866: return "KinDynComputations_getCentroidalAverageVelocity";
+  case 1867: return "KinDynComputations_getCentroidalAverageVelocityJacobian";
+  case 1868: return "KinDynComputations_getLinearAngularMomentum";
+  case 1869: return "KinDynComputations_getLinearAngularMomentumJacobian";
+  case 1870: return "KinDynComputations_getCentroidalTotalMomentum";
+  case 1871: return "KinDynComputations_getFreeFloatingMassMatrix";
+  case 1872: return "KinDynComputations_inverseDynamics";
+  case 1873: return "KinDynComputations_generalizedBiasForces";
+  case 1874: return "KinDynComputations_generalizedGravityForces";
+  case 1875: return "KinDynComputations_generalizedExternalForces";
+  case 1876: return "KinDynComputations_inverseDynamicsInertialParametersRegressor";
+  case 1877: return "delete_ICamera";
+  case 1878: return "ICamera_setPosition";
+  case 1879: return "ICamera_setTarget";
+  case 1880: return "ICamera_setUpVector";
+  case 1881: return "ColorViz_r_get";
+  case 1882: return "ColorViz_r_set";
+  case 1883: return "ColorViz_g_get";
+  case 1884: return "ColorViz_g_set";
+  case 1885: return "ColorViz_b_get";
+  case 1886: return "ColorViz_b_set";
+  case 1887: return "ColorViz_a_get";
+  case 1888: return "ColorViz_a_set";
+  case 1889: return "new_ColorViz";
+  case 1890: return "delete_ColorViz";
+  case 1891: return "delete_ILight";
+  case 1892: return "ILight_getName";
+  case 1893: return "ILight_setType";
+  case 1894: return "ILight_getType";
+  case 1895: return "ILight_setPosition";
+  case 1896: return "ILight_getPosition";
+  case 1897: return "ILight_setDirection";
+  case 1898: return "ILight_getDirection";
+  case 1899: return "ILight_setAmbientColor";
+  case 1900: return "ILight_getAmbientColor";
+  case 1901: return "ILight_setSpecularColor";
+  case 1902: return "ILight_getSpecularColor";
+  case 1903: return "ILight_setDiffuseColor";
+  case 1904: return "ILight_getDiffuseColor";
+  case 1905: return "delete_IEnvironment";
+  case 1906: return "IEnvironment_getElements";
+  case 1907: return "IEnvironment_setElementVisibility";
+  case 1908: return "IEnvironment_setBackgroundColor";
+  case 1909: return "IEnvironment_setAmbientLight";
+  case 1910: return "IEnvironment_getLights";
+  case 1911: return "IEnvironment_addLight";
+  case 1912: return "IEnvironment_lightViz";
+  case 1913: return "IEnvironment_removeLight";
+  case 1914: return "delete_IJetsVisualization";
+  case 1915: return "IJetsVisualization_setJetsFrames";
+  case 1916: return "IJetsVisualization_getNrOfJets";
+  case 1917: return "IJetsVisualization_getJetDirection";
+  case 1918: return "IJetsVisualization_setJetDirection";
+  case 1919: return "IJetsVisualization_setJetColor";
+  case 1920: return "IJetsVisualization_setJetsDimensions";
+  case 1921: return "IJetsVisualization_setJetsIntensity";
+  case 1922: return "delete_IVectorsVisualization";
+  case 1923: return "IVectorsVisualization_addVector";
+  case 1924: return "IVectorsVisualization_getNrOfVectors";
+  case 1925: return "IVectorsVisualization_getVector";
+  case 1926: return "IVectorsVisualization_updateVector";
+  case 1927: return "IVectorsVisualization_setVectorColor";
+  case 1928: return "IVectorsVisualization_setVectorsAspect";
+  case 1929: return "delete_IModelVisualization";
+  case 1930: return "IModelVisualization_setPositions";
+  case 1931: return "IModelVisualization_setLinkPositions";
+  case 1932: return "IModelVisualization_model";
+  case 1933: return "IModelVisualization_getInstanceName";
+  case 1934: return "IModelVisualization_setModelVisibility";
+  case 1935: return "IModelVisualization_setModelColor";
+  case 1936: return "IModelVisualization_resetModelColor";
+  case 1937: return "IModelVisualization_setLinkColor";
+  case 1938: return "IModelVisualization_resetLinkColor";
+  case 1939: return "IModelVisualization_getLinkNames";
+  case 1940: return "IModelVisualization_setLinkVisibility";
+  case 1941: return "IModelVisualization_getFeatures";
+  case 1942: return "IModelVisualization_setFeatureVisibility";
+  case 1943: return "IModelVisualization_jets";
+  case 1944: return "IModelVisualization_getWorldModelTransform";
+  case 1945: return "IModelVisualization_getWorldLinkTransform";
+  case 1946: return "VisualizerOptions_verbose_get";
+  case 1947: return "VisualizerOptions_verbose_set";
+  case 1948: return "VisualizerOptions_winWidth_get";
+  case 1949: return "VisualizerOptions_winWidth_set";
+  case 1950: return "VisualizerOptions_winHeight_get";
+  case 1951: return "VisualizerOptions_winHeight_set";
+  case 1952: return "VisualizerOptions_rootFrameArrowsDimension_get";
+  case 1953: return "VisualizerOptions_rootFrameArrowsDimension_set";
+  case 1954: return "new_VisualizerOptions";
+  case 1955: return "delete_VisualizerOptions";
+  case 1956: return "new_Visualizer";
+  case 1957: return "delete_Visualizer";
+  case 1958: return "Visualizer_init";
+  case 1959: return "Visualizer_getNrOfVisualizedModels";
+  case 1960: return "Visualizer_getModelInstanceName";
+  case 1961: return "Visualizer_getModelInstanceIndex";
+  case 1962: return "Visualizer_addModel";
+  case 1963: return "Visualizer_modelViz";
+  case 1964: return "Visualizer_camera";
+  case 1965: return "Visualizer_enviroment";
+  case 1966: return "Visualizer_vectors";
+  case 1967: return "Visualizer_run";
+  case 1968: return "Visualizer_draw";
+  case 1969: return "Visualizer_drawToFile";
+  case 1970: return "Visualizer_close";
+  case 1971: return "new_DynamicsComputations";
+  case 1972: return "delete_DynamicsComputations";
+  case 1973: return "DynamicsComputations_loadRobotModelFromFile";
+  case 1974: return "DynamicsComputations_loadRobotModelFromString";
+  case 1975: return "DynamicsComputations_isValid";
+  case 1976: return "DynamicsComputations_getNrOfDegreesOfFreedom";
+  case 1977: return "DynamicsComputations_getDescriptionOfDegreeOfFreedom";
+  case 1978: return "DynamicsComputations_getDescriptionOfDegreesOfFreedom";
+  case 1979: return "DynamicsComputations_getNrOfLinks";
+  case 1980: return "DynamicsComputations_getNrOfFrames";
+  case 1981: return "DynamicsComputations_getFloatingBase";
+  case 1982: return "DynamicsComputations_setFloatingBase";
+  case 1983: return "DynamicsComputations_setRobotState";
+  case 1984: return "DynamicsComputations_getWorldBaseTransform";
+  case 1985: return "DynamicsComputations_getBaseTwist";
+  case 1986: return "DynamicsComputations_getJointPos";
+  case 1987: return "DynamicsComputations_getJointVel";
+  case 1988: return "DynamicsComputations_getFrameIndex";
+  case 1989: return "DynamicsComputations_getFrameName";
+  case 1990: return "DynamicsComputations_getWorldTransform";
+  case 1991: return "DynamicsComputations_getRelativeTransform";
+  case 1992: return "DynamicsComputations_getFrameTwist";
+  case 1993: return "DynamicsComputations_getFrameTwistInWorldOrient";
+  case 1994: return "DynamicsComputations_getFrameProperSpatialAcceleration";
+  case 1995: return "DynamicsComputations_getLinkIndex";
+  case 1996: return "DynamicsComputations_getLinkInertia";
+  case 1997: return "DynamicsComputations_getJointIndex";
+  case 1998: return "DynamicsComputations_getJointName";
+  case 1999: return "DynamicsComputations_getJointLimits";
+  case 2000: return "DynamicsComputations_inverseDynamics";
+  case 2001: return "DynamicsComputations_getFreeFloatingMassMatrix";
+  case 2002: return "DynamicsComputations_getFrameJacobian";
+  case 2003: return "DynamicsComputations_getDynamicsRegressor";
+  case 2004: return "DynamicsComputations_getModelDynamicsParameters";
+  case 2005: return "DynamicsComputations_getCenterOfMass";
+  case 2006: return "DynamicsComputations_getCenterOfMassJacobian";
   default: return 0;
   }
 }
@@ -98086,1219 +99819,1245 @@ void mexFunction(int resc, mxArray *resv[], int argc, const mxArray *argv[]) {
   case 765: flag=_wrap_Rotation_RotAxisDerivative(resc,resv,argc,(mxArray**)(argv)); break;
   case 766: flag=_wrap_Rotation_RPY(resc,resv,argc,(mxArray**)(argv)); break;
   case 767: flag=_wrap_Rotation_RPYRightTrivializedDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 768: flag=_wrap_Rotation_RPYRightTrivializedDerivativeInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 769: flag=_wrap_Rotation_QuaternionRightTrivializedDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 770: flag=_wrap_Rotation_QuaternionRightTrivializedDerivativeInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 771: flag=_wrap_Rotation_Identity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 772: flag=_wrap_Rotation_RotationFromQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 773: flag=_wrap_Rotation_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 774: flag=_wrap_Rotation_display(resc,resv,argc,(mxArray**)(argv)); break;
-  case 775: flag=_wrap_delete_Rotation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 776: flag=_wrap_new_TransformSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 777: flag=_wrap_TransformSemantics_getRotationSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 778: flag=_wrap_TransformSemantics_getPositionSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 779: flag=_wrap_TransformSemantics_setRotationSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 780: flag=_wrap_TransformSemantics_setPositionSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 781: flag=_wrap_TransformSemantics_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 782: flag=_wrap_TransformSemantics_display(resc,resv,argc,(mxArray**)(argv)); break;
-  case 783: flag=_wrap_delete_TransformSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 784: flag=_wrap_new_Transform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 785: flag=_wrap_Transform_fromHomogeneousTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 786: flag=_wrap_Transform_getSemantics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 787: flag=_wrap_Transform_getRotation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 788: flag=_wrap_Transform_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 789: flag=_wrap_Transform_setRotation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 790: flag=_wrap_Transform_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 791: flag=_wrap_Transform_compose(resc,resv,argc,(mxArray**)(argv)); break;
-  case 792: flag=_wrap_Transform_inverse2(resc,resv,argc,(mxArray**)(argv)); break;
-  case 793: flag=_wrap_Transform_inverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 794: flag=_wrap_Transform_mtimes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 795: flag=_wrap_Transform_Identity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 796: flag=_wrap_Transform_asHomogeneousTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 797: flag=_wrap_Transform_asAdjointTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 798: flag=_wrap_Transform_asAdjointTransformWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 799: flag=_wrap_Transform_log(resc,resv,argc,(mxArray**)(argv)); break;
-  case 800: flag=_wrap_Transform_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 801: flag=_wrap_Transform_display(resc,resv,argc,(mxArray**)(argv)); break;
-  case 802: flag=_wrap_delete_Transform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 803: flag=_wrap_new_TransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 804: flag=_wrap_delete_TransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 805: flag=_wrap_TransformDerivative_getRotationDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 806: flag=_wrap_TransformDerivative_getPositionDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 807: flag=_wrap_TransformDerivative_setRotationDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 808: flag=_wrap_TransformDerivative_setPositionDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 809: flag=_wrap_TransformDerivative_Zero(resc,resv,argc,(mxArray**)(argv)); break;
-  case 810: flag=_wrap_TransformDerivative_asHomogeneousTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 811: flag=_wrap_TransformDerivative_asAdjointTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 812: flag=_wrap_TransformDerivative_asAdjointTransformWrenchDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 813: flag=_wrap_TransformDerivative_mtimes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 814: flag=_wrap_TransformDerivative_derivativeOfInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 815: flag=_wrap_TransformDerivative_transform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 816: flag=_wrap_dynamic_extent_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 817: flag=_wrap_DynamicSpan_extent_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 818: flag=_wrap_new_DynamicSpan(resc,resv,argc,(mxArray**)(argv)); break;
-  case 819: flag=_wrap_delete_DynamicSpan(resc,resv,argc,(mxArray**)(argv)); break;
-  case 820: flag=_wrap_DynamicSpan_first(resc,resv,argc,(mxArray**)(argv)); break;
-  case 821: flag=_wrap_DynamicSpan_last(resc,resv,argc,(mxArray**)(argv)); break;
-  case 822: flag=_wrap_DynamicSpan_subspan(resc,resv,argc,(mxArray**)(argv)); break;
-  case 823: flag=_wrap_DynamicSpan_size(resc,resv,argc,(mxArray**)(argv)); break;
-  case 824: flag=_wrap_DynamicSpan_size_bytes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 825: flag=_wrap_DynamicSpan_empty(resc,resv,argc,(mxArray**)(argv)); break;
-  case 826: flag=_wrap_DynamicSpan_brace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 827: flag=_wrap_DynamicSpan_getVal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 828: flag=_wrap_DynamicSpan_setVal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 829: flag=_wrap_DynamicSpan_at(resc,resv,argc,(mxArray**)(argv)); break;
-  case 830: flag=_wrap_DynamicSpan_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 831: flag=_wrap_DynamicSpan_begin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 832: flag=_wrap_DynamicSpan_end(resc,resv,argc,(mxArray**)(argv)); break;
-  case 833: flag=_wrap_DynamicSpan_cbegin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 834: flag=_wrap_DynamicSpan_cend(resc,resv,argc,(mxArray**)(argv)); break;
-  case 835: flag=_wrap_DynamicSpan_rbegin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 836: flag=_wrap_DynamicSpan_rend(resc,resv,argc,(mxArray**)(argv)); break;
-  case 837: flag=_wrap_DynamicSpan_crbegin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 838: flag=_wrap_DynamicSpan_crend(resc,resv,argc,(mxArray**)(argv)); break;
-  case 839: flag=_wrap_LINK_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 840: flag=_wrap_LINK_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 841: flag=_wrap_LINK_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 842: flag=_wrap_LINK_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 843: flag=_wrap_JOINT_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 844: flag=_wrap_JOINT_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 845: flag=_wrap_JOINT_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 846: flag=_wrap_JOINT_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 847: flag=_wrap_DOF_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 848: flag=_wrap_DOF_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 849: flag=_wrap_DOF_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 850: flag=_wrap_DOF_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 851: flag=_wrap_FRAME_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 852: flag=_wrap_FRAME_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 853: flag=_wrap_FRAME_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 854: flag=_wrap_FRAME_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 855: flag=_wrap_TRAVERSAL_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 856: flag=_wrap_TRAVERSAL_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 857: flag=_wrap_new_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 858: flag=_wrap_LinkPositions_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 859: flag=_wrap_LinkPositions_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 860: flag=_wrap_LinkPositions_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 861: flag=_wrap_LinkPositions_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 862: flag=_wrap_LinkPositions_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 863: flag=_wrap_delete_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 864: flag=_wrap_new_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 865: flag=_wrap_LinkWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 866: flag=_wrap_LinkWrenches_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 867: flag=_wrap_LinkWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 868: flag=_wrap_LinkWrenches_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 869: flag=_wrap_LinkWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 870: flag=_wrap_LinkWrenches_zero(resc,resv,argc,(mxArray**)(argv)); break;
-  case 871: flag=_wrap_delete_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 872: flag=_wrap_new_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 873: flag=_wrap_LinkInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 874: flag=_wrap_LinkInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 875: flag=_wrap_LinkInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 876: flag=_wrap_delete_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 877: flag=_wrap_new_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 878: flag=_wrap_LinkArticulatedBodyInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 879: flag=_wrap_LinkArticulatedBodyInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 880: flag=_wrap_LinkArticulatedBodyInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 881: flag=_wrap_delete_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 882: flag=_wrap_new_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 883: flag=_wrap_LinkVelArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 884: flag=_wrap_LinkVelArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 885: flag=_wrap_LinkVelArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 886: flag=_wrap_LinkVelArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 887: flag=_wrap_LinkVelArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 888: flag=_wrap_delete_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 889: flag=_wrap_new_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 890: flag=_wrap_LinkAccArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 891: flag=_wrap_LinkAccArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 892: flag=_wrap_LinkAccArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 893: flag=_wrap_LinkAccArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 894: flag=_wrap_LinkAccArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 895: flag=_wrap_delete_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 896: flag=_wrap_new_Link(resc,resv,argc,(mxArray**)(argv)); break;
-  case 897: flag=_wrap_Link_inertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 898: flag=_wrap_Link_setInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 899: flag=_wrap_Link_getInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 900: flag=_wrap_Link_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 901: flag=_wrap_Link_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 902: flag=_wrap_delete_Link(resc,resv,argc,(mxArray**)(argv)); break;
-  case 903: flag=_wrap_delete_IJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 904: flag=_wrap_IJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 905: flag=_wrap_IJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 906: flag=_wrap_IJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 907: flag=_wrap_IJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 908: flag=_wrap_IJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 909: flag=_wrap_IJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 910: flag=_wrap_IJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 911: flag=_wrap_IJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 912: flag=_wrap_IJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 913: flag=_wrap_IJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 914: flag=_wrap_IJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 915: flag=_wrap_IJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 916: flag=_wrap_IJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 917: flag=_wrap_IJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 918: flag=_wrap_IJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 919: flag=_wrap_IJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 920: flag=_wrap_IJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 921: flag=_wrap_IJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 922: flag=_wrap_IJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 923: flag=_wrap_IJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 924: flag=_wrap_IJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 925: flag=_wrap_IJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 926: flag=_wrap_IJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 927: flag=_wrap_IJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 928: flag=_wrap_IJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 929: flag=_wrap_IJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 930: flag=_wrap_IJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 931: flag=_wrap_IJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 932: flag=_wrap_IJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 933: flag=_wrap_IJoint_isRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 934: flag=_wrap_IJoint_isFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 935: flag=_wrap_IJoint_asRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 936: flag=_wrap_IJoint_asFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 937: flag=_wrap_new_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 938: flag=_wrap_delete_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 939: flag=_wrap_FixedJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 940: flag=_wrap_FixedJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 941: flag=_wrap_FixedJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 942: flag=_wrap_FixedJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 943: flag=_wrap_FixedJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 944: flag=_wrap_FixedJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 945: flag=_wrap_FixedJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 946: flag=_wrap_FixedJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 947: flag=_wrap_FixedJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 948: flag=_wrap_FixedJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 949: flag=_wrap_FixedJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 950: flag=_wrap_FixedJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 951: flag=_wrap_FixedJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 952: flag=_wrap_FixedJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 953: flag=_wrap_FixedJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 954: flag=_wrap_FixedJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 955: flag=_wrap_FixedJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 956: flag=_wrap_FixedJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 957: flag=_wrap_FixedJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 958: flag=_wrap_FixedJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 959: flag=_wrap_FixedJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 960: flag=_wrap_FixedJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 961: flag=_wrap_FixedJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 962: flag=_wrap_FixedJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 963: flag=_wrap_FixedJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 964: flag=_wrap_FixedJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 965: flag=_wrap_FixedJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 966: flag=_wrap_FixedJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 967: flag=_wrap_FixedJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 968: flag=_wrap_delete_MovableJointImpl1(resc,resv,argc,(mxArray**)(argv)); break;
-  case 969: flag=_wrap_MovableJointImpl1_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 970: flag=_wrap_MovableJointImpl1_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 971: flag=_wrap_MovableJointImpl1_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 972: flag=_wrap_MovableJointImpl1_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 973: flag=_wrap_MovableJointImpl1_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 974: flag=_wrap_MovableJointImpl1_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 975: flag=_wrap_MovableJointImpl1_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 976: flag=_wrap_MovableJointImpl1_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 977: flag=_wrap_delete_MovableJointImpl2(resc,resv,argc,(mxArray**)(argv)); break;
-  case 978: flag=_wrap_MovableJointImpl2_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 979: flag=_wrap_MovableJointImpl2_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 980: flag=_wrap_MovableJointImpl2_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 981: flag=_wrap_MovableJointImpl2_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 982: flag=_wrap_MovableJointImpl2_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 983: flag=_wrap_MovableJointImpl2_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 984: flag=_wrap_MovableJointImpl2_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 985: flag=_wrap_MovableJointImpl2_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 986: flag=_wrap_delete_MovableJointImpl3(resc,resv,argc,(mxArray**)(argv)); break;
-  case 987: flag=_wrap_MovableJointImpl3_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 988: flag=_wrap_MovableJointImpl3_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 989: flag=_wrap_MovableJointImpl3_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 990: flag=_wrap_MovableJointImpl3_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 991: flag=_wrap_MovableJointImpl3_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 992: flag=_wrap_MovableJointImpl3_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 993: flag=_wrap_MovableJointImpl3_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 994: flag=_wrap_MovableJointImpl3_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 995: flag=_wrap_delete_MovableJointImpl4(resc,resv,argc,(mxArray**)(argv)); break;
-  case 996: flag=_wrap_MovableJointImpl4_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 997: flag=_wrap_MovableJointImpl4_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 998: flag=_wrap_MovableJointImpl4_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 999: flag=_wrap_MovableJointImpl4_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1000: flag=_wrap_MovableJointImpl4_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1001: flag=_wrap_MovableJointImpl4_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1002: flag=_wrap_MovableJointImpl4_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1003: flag=_wrap_MovableJointImpl4_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1004: flag=_wrap_delete_MovableJointImpl5(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1005: flag=_wrap_MovableJointImpl5_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1006: flag=_wrap_MovableJointImpl5_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1007: flag=_wrap_MovableJointImpl5_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1008: flag=_wrap_MovableJointImpl5_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1009: flag=_wrap_MovableJointImpl5_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1010: flag=_wrap_MovableJointImpl5_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1011: flag=_wrap_MovableJointImpl5_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1012: flag=_wrap_MovableJointImpl5_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1013: flag=_wrap_delete_MovableJointImpl6(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1014: flag=_wrap_MovableJointImpl6_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1015: flag=_wrap_MovableJointImpl6_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1016: flag=_wrap_MovableJointImpl6_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1017: flag=_wrap_MovableJointImpl6_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1018: flag=_wrap_MovableJointImpl6_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1019: flag=_wrap_MovableJointImpl6_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1020: flag=_wrap_MovableJointImpl6_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1021: flag=_wrap_MovableJointImpl6_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1022: flag=_wrap_new_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1023: flag=_wrap_delete_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1024: flag=_wrap_RevoluteJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1025: flag=_wrap_RevoluteJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1026: flag=_wrap_RevoluteJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1027: flag=_wrap_RevoluteJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1028: flag=_wrap_RevoluteJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1029: flag=_wrap_RevoluteJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1030: flag=_wrap_RevoluteJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1031: flag=_wrap_RevoluteJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1032: flag=_wrap_RevoluteJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1033: flag=_wrap_RevoluteJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1034: flag=_wrap_RevoluteJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1035: flag=_wrap_RevoluteJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1036: flag=_wrap_RevoluteJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1037: flag=_wrap_RevoluteJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1038: flag=_wrap_RevoluteJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1039: flag=_wrap_RevoluteJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1040: flag=_wrap_RevoluteJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1041: flag=_wrap_RevoluteJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1042: flag=_wrap_RevoluteJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1043: flag=_wrap_RevoluteJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1044: flag=_wrap_RevoluteJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1045: flag=_wrap_RevoluteJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1046: flag=_wrap_RevoluteJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1047: flag=_wrap_new_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1048: flag=_wrap_delete_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1049: flag=_wrap_PrismaticJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1050: flag=_wrap_PrismaticJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1051: flag=_wrap_PrismaticJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1052: flag=_wrap_PrismaticJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1053: flag=_wrap_PrismaticJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1054: flag=_wrap_PrismaticJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1055: flag=_wrap_PrismaticJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1056: flag=_wrap_PrismaticJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1057: flag=_wrap_PrismaticJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1058: flag=_wrap_PrismaticJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1059: flag=_wrap_PrismaticJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1060: flag=_wrap_PrismaticJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1061: flag=_wrap_PrismaticJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1062: flag=_wrap_PrismaticJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1063: flag=_wrap_PrismaticJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1064: flag=_wrap_PrismaticJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1065: flag=_wrap_PrismaticJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1066: flag=_wrap_PrismaticJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1067: flag=_wrap_PrismaticJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1068: flag=_wrap_PrismaticJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1069: flag=_wrap_PrismaticJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1070: flag=_wrap_PrismaticJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1071: flag=_wrap_PrismaticJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1072: flag=_wrap_new_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1073: flag=_wrap_delete_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1074: flag=_wrap_Traversal_getNrOfVisitedLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1075: flag=_wrap_Traversal_getLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1076: flag=_wrap_Traversal_getBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1077: flag=_wrap_Traversal_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1078: flag=_wrap_Traversal_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1079: flag=_wrap_Traversal_getParentLinkFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1080: flag=_wrap_Traversal_getParentJointFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1081: flag=_wrap_Traversal_getTraversalIndexFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1082: flag=_wrap_Traversal_reset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1083: flag=_wrap_Traversal_addTraversalBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1084: flag=_wrap_Traversal_addTraversalElement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1085: flag=_wrap_Traversal_isParentOf(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1086: flag=_wrap_Traversal_getChildLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1087: flag=_wrap_Traversal_getParentLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1088: flag=_wrap_Traversal_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1089: flag=_wrap_delete_SolidShape(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1090: flag=_wrap_SolidShape_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1091: flag=_wrap_SolidShape_name_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1092: flag=_wrap_SolidShape_name_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1093: flag=_wrap_SolidShape_link_H_geometry_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1094: flag=_wrap_SolidShape_link_H_geometry_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1095: flag=_wrap_SolidShape_material_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1096: flag=_wrap_SolidShape_material_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1097: flag=_wrap_SolidShape_isSphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1098: flag=_wrap_SolidShape_isBox(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1099: flag=_wrap_SolidShape_isCylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1100: flag=_wrap_SolidShape_isExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1101: flag=_wrap_SolidShape_asSphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1102: flag=_wrap_SolidShape_asBox(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1103: flag=_wrap_SolidShape_asCylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1104: flag=_wrap_SolidShape_asExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1105: flag=_wrap_delete_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1106: flag=_wrap_Sphere_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1107: flag=_wrap_Sphere_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1108: flag=_wrap_Sphere_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1109: flag=_wrap_new_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1110: flag=_wrap_delete_Box(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1111: flag=_wrap_Box_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1112: flag=_wrap_Box_x_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1113: flag=_wrap_Box_x_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1114: flag=_wrap_Box_y_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1115: flag=_wrap_Box_y_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1116: flag=_wrap_Box_z_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1117: flag=_wrap_Box_z_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1118: flag=_wrap_new_Box(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1119: flag=_wrap_delete_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1120: flag=_wrap_Cylinder_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1121: flag=_wrap_Cylinder_length_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1122: flag=_wrap_Cylinder_length_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1123: flag=_wrap_Cylinder_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1124: flag=_wrap_Cylinder_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1125: flag=_wrap_new_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1126: flag=_wrap_delete_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1127: flag=_wrap_ExternalMesh_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1128: flag=_wrap_ExternalMesh_filename_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1129: flag=_wrap_ExternalMesh_filename_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1130: flag=_wrap_ExternalMesh_scale_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1131: flag=_wrap_ExternalMesh_scale_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1132: flag=_wrap_new_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1133: flag=_wrap_new_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1134: flag=_wrap_ModelSolidShapes_clear(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1135: flag=_wrap_delete_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1136: flag=_wrap_ModelSolidShapes_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1137: flag=_wrap_ModelSolidShapes_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1138: flag=_wrap_ModelSolidShapes_linkSolidShapes_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1139: flag=_wrap_ModelSolidShapes_linkSolidShapes_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1140: flag=_wrap_Neighbor_neighborLink_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1141: flag=_wrap_Neighbor_neighborLink_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1142: flag=_wrap_Neighbor_neighborJoint_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1143: flag=_wrap_Neighbor_neighborJoint_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1144: flag=_wrap_new_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1145: flag=_wrap_delete_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1146: flag=_wrap_new_Model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1147: flag=_wrap_Model_copy(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1148: flag=_wrap_delete_Model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1149: flag=_wrap_Model_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1150: flag=_wrap_Model_getLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1151: flag=_wrap_Model_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1152: flag=_wrap_Model_isValidLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1153: flag=_wrap_Model_getLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1154: flag=_wrap_Model_addLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1155: flag=_wrap_Model_getNrOfJoints(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1156: flag=_wrap_Model_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1157: flag=_wrap_Model_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1158: flag=_wrap_Model_getJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1159: flag=_wrap_Model_isValidJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1160: flag=_wrap_Model_isLinkNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1161: flag=_wrap_Model_isJointNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1162: flag=_wrap_Model_isFrameNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1163: flag=_wrap_Model_addJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1164: flag=_wrap_Model_addJointAndLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1165: flag=_wrap_Model_insertLinkToExistingJointAndAddJointForDisplacedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1166: flag=_wrap_Model_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1167: flag=_wrap_Model_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1168: flag=_wrap_Model_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1169: flag=_wrap_Model_addAdditionalFrameToLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1170: flag=_wrap_Model_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1171: flag=_wrap_Model_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1172: flag=_wrap_Model_isValidFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1173: flag=_wrap_Model_getFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1174: flag=_wrap_Model_getFrameLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1175: flag=_wrap_Model_getNrOfNeighbors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1176: flag=_wrap_Model_getNeighbor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1177: flag=_wrap_Model_setDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1178: flag=_wrap_Model_getDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1179: flag=_wrap_Model_computeFullTreeTraversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1180: flag=_wrap_Model_getInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1181: flag=_wrap_Model_updateInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1182: flag=_wrap_Model_visualSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1183: flag=_wrap_Model_collisionSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1184: flag=_wrap_Model_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1185: flag=_wrap_new_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1186: flag=_wrap_JointPosDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1187: flag=_wrap_JointPosDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1188: flag=_wrap_delete_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1189: flag=_wrap_new_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1190: flag=_wrap_JointDOFsDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1191: flag=_wrap_JointDOFsDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1192: flag=_wrap_delete_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1193: flag=_wrap_new_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1194: flag=_wrap_DOFSpatialForceArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1195: flag=_wrap_DOFSpatialForceArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1196: flag=_wrap_DOFSpatialForceArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1197: flag=_wrap_delete_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1198: flag=_wrap_new_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1199: flag=_wrap_DOFSpatialMotionArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1200: flag=_wrap_DOFSpatialMotionArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1201: flag=_wrap_DOFSpatialMotionArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1202: flag=_wrap_delete_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1203: flag=_wrap_new_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1204: flag=_wrap_FrameFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1205: flag=_wrap_FrameFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1206: flag=_wrap_delete_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1207: flag=_wrap_new_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1208: flag=_wrap_MomentumFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1209: flag=_wrap_MomentumFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1210: flag=_wrap_delete_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1211: flag=_wrap_new_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1212: flag=_wrap_FreeFloatingMassMatrix_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1213: flag=_wrap_delete_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1214: flag=_wrap_new_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1215: flag=_wrap_FreeFloatingPos_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1216: flag=_wrap_FreeFloatingPos_worldBasePos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1217: flag=_wrap_FreeFloatingPos_jointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1218: flag=_wrap_FreeFloatingPos_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1219: flag=_wrap_delete_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1220: flag=_wrap_new_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1221: flag=_wrap_FreeFloatingGeneralizedTorques_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1222: flag=_wrap_FreeFloatingGeneralizedTorques_baseWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1223: flag=_wrap_FreeFloatingGeneralizedTorques_jointTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1224: flag=_wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1225: flag=_wrap_delete_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1226: flag=_wrap_new_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1227: flag=_wrap_FreeFloatingVel_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1228: flag=_wrap_FreeFloatingVel_baseVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1229: flag=_wrap_FreeFloatingVel_jointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1230: flag=_wrap_FreeFloatingVel_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1231: flag=_wrap_delete_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1232: flag=_wrap_new_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1233: flag=_wrap_FreeFloatingAcc_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1234: flag=_wrap_FreeFloatingAcc_baseAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1235: flag=_wrap_FreeFloatingAcc_jointAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1236: flag=_wrap_FreeFloatingAcc_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1237: flag=_wrap_delete_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1238: flag=_wrap_ContactWrench_contactId(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1239: flag=_wrap_ContactWrench_contactPoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1240: flag=_wrap_ContactWrench_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1241: flag=_wrap_new_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1242: flag=_wrap_delete_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1243: flag=_wrap_new_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1244: flag=_wrap_LinkContactWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1245: flag=_wrap_LinkContactWrenches_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1246: flag=_wrap_LinkContactWrenches_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1247: flag=_wrap_LinkContactWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1248: flag=_wrap_LinkContactWrenches_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1249: flag=_wrap_LinkContactWrenches_computeNetWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1250: flag=_wrap_LinkContactWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1251: flag=_wrap_delete_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1252: flag=_wrap_ForwardPositionKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1253: flag=_wrap_ForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1254: flag=_wrap_ForwardPosVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1255: flag=_wrap_ForwardPosVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1256: flag=_wrap_ForwardAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1257: flag=_wrap_ForwardBiasAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1258: flag=_wrap_ComputeLinearAndAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1259: flag=_wrap_ComputeLinearAndAngularMomentumDerivativeBias(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1260: flag=_wrap_RNEADynamicPhase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1261: flag=_wrap_CompositeRigidBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1262: flag=_wrap_new_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1263: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1264: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1265: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1266: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1267: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1268: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1269: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1270: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1271: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1272: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1273: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1274: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1275: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1276: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1277: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1278: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1279: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1280: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1281: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1282: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1283: flag=_wrap_delete_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1284: flag=_wrap_ArticulatedBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1285: flag=_wrap_InverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1286: flag=_wrap_NR_OF_SENSOR_TYPES_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1287: flag=_wrap_isLinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1288: flag=_wrap_isJointSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1289: flag=_wrap_getSensorTypeSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1290: flag=_wrap_delete_Sensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1291: flag=_wrap_Sensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1292: flag=_wrap_Sensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1293: flag=_wrap_Sensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1294: flag=_wrap_Sensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1295: flag=_wrap_Sensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1296: flag=_wrap_Sensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1297: flag=_wrap_Sensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1298: flag=_wrap_Sensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1299: flag=_wrap_delete_JointSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1300: flag=_wrap_JointSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1301: flag=_wrap_JointSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1302: flag=_wrap_JointSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1303: flag=_wrap_JointSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1304: flag=_wrap_JointSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1305: flag=_wrap_delete_LinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1306: flag=_wrap_LinkSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1307: flag=_wrap_LinkSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1308: flag=_wrap_LinkSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1309: flag=_wrap_LinkSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1310: flag=_wrap_LinkSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1311: flag=_wrap_LinkSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1312: flag=_wrap_LinkSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1313: flag=_wrap_new_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1314: flag=_wrap_delete_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1315: flag=_wrap_SensorsList_addSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1316: flag=_wrap_SensorsList_setSerialization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1317: flag=_wrap_SensorsList_getSerialization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1318: flag=_wrap_SensorsList_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1319: flag=_wrap_SensorsList_getSensorIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1320: flag=_wrap_SensorsList_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1321: flag=_wrap_SensorsList_getSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1322: flag=_wrap_SensorsList_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1323: flag=_wrap_SensorsList_removeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1324: flag=_wrap_SensorsList_removeAllSensorsOfType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1325: flag=_wrap_SensorsList_getSixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1326: flag=_wrap_SensorsList_getAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1327: flag=_wrap_SensorsList_getGyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1328: flag=_wrap_SensorsList_getThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1329: flag=_wrap_SensorsList_getThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1330: flag=_wrap_new_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1331: flag=_wrap_delete_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1332: flag=_wrap_SensorsMeasurements_setNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1333: flag=_wrap_SensorsMeasurements_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1334: flag=_wrap_SensorsMeasurements_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1335: flag=_wrap_SensorsMeasurements_toVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1336: flag=_wrap_SensorsMeasurements_setMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1337: flag=_wrap_SensorsMeasurements_getMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1338: flag=_wrap_SensorsMeasurements_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1339: flag=_wrap_new_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1340: flag=_wrap_delete_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1341: flag=_wrap_SixAxisForceTorqueSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1342: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1343: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1344: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1345: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1346: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1347: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1348: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1349: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1350: flag=_wrap_SixAxisForceTorqueSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1351: flag=_wrap_SixAxisForceTorqueSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1352: flag=_wrap_SixAxisForceTorqueSensor_setAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1353: flag=_wrap_SixAxisForceTorqueSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1354: flag=_wrap_SixAxisForceTorqueSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1355: flag=_wrap_SixAxisForceTorqueSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1356: flag=_wrap_SixAxisForceTorqueSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1357: flag=_wrap_SixAxisForceTorqueSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1358: flag=_wrap_SixAxisForceTorqueSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1359: flag=_wrap_SixAxisForceTorqueSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1360: flag=_wrap_SixAxisForceTorqueSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1361: flag=_wrap_SixAxisForceTorqueSensor_getAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1362: flag=_wrap_SixAxisForceTorqueSensor_isLinkAttachedToSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1363: flag=_wrap_SixAxisForceTorqueSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1364: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1365: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1366: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1367: flag=_wrap_SixAxisForceTorqueSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1368: flag=_wrap_SixAxisForceTorqueSensor_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1369: flag=_wrap_new_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1370: flag=_wrap_delete_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1371: flag=_wrap_AccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1372: flag=_wrap_AccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1373: flag=_wrap_AccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1374: flag=_wrap_AccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1375: flag=_wrap_AccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1376: flag=_wrap_AccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1377: flag=_wrap_AccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1378: flag=_wrap_AccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1379: flag=_wrap_AccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1380: flag=_wrap_AccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1381: flag=_wrap_AccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1382: flag=_wrap_AccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1383: flag=_wrap_AccelerometerSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1384: flag=_wrap_AccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1385: flag=_wrap_new_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1386: flag=_wrap_delete_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1387: flag=_wrap_GyroscopeSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1388: flag=_wrap_GyroscopeSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1389: flag=_wrap_GyroscopeSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1390: flag=_wrap_GyroscopeSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1391: flag=_wrap_GyroscopeSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1392: flag=_wrap_GyroscopeSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1393: flag=_wrap_GyroscopeSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1394: flag=_wrap_GyroscopeSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1395: flag=_wrap_GyroscopeSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1396: flag=_wrap_GyroscopeSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1397: flag=_wrap_GyroscopeSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1398: flag=_wrap_GyroscopeSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1399: flag=_wrap_GyroscopeSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1400: flag=_wrap_GyroscopeSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1401: flag=_wrap_new_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1402: flag=_wrap_delete_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1403: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1404: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1405: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1406: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1407: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1408: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1409: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1410: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1411: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1412: flag=_wrap_ThreeAxisAngularAccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1413: flag=_wrap_ThreeAxisAngularAccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1414: flag=_wrap_ThreeAxisAngularAccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1415: flag=_wrap_ThreeAxisAngularAccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1416: flag=_wrap_new_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1417: flag=_wrap_delete_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1418: flag=_wrap_ThreeAxisForceTorqueContactSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1419: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1420: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1421: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1422: flag=_wrap_ThreeAxisForceTorqueContactSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1423: flag=_wrap_ThreeAxisForceTorqueContactSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1424: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1425: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1426: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1427: flag=_wrap_ThreeAxisForceTorqueContactSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1428: flag=_wrap_ThreeAxisForceTorqueContactSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1429: flag=_wrap_ThreeAxisForceTorqueContactSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1430: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1431: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1432: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1433: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1434: flag=_wrap_predictSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1435: flag=_wrap_predictSensorsMeasurementsFromRawBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1436: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1437: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1438: flag=_wrap_URDFParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1439: flag=_wrap_URDFParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1440: flag=_wrap_new_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1441: flag=_wrap_delete_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1442: flag=_wrap_modelFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1443: flag=_wrap_modelFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1444: flag=_wrap_dofsListFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1445: flag=_wrap_dofsListFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1446: flag=_wrap_sensorsFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1447: flag=_wrap_sensorsFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1448: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1449: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1450: flag=_wrap_ModelParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1451: flag=_wrap_ModelParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1452: flag=_wrap_new_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1453: flag=_wrap_delete_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1454: flag=_wrap_new_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1455: flag=_wrap_delete_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1456: flag=_wrap_ModelLoader_parsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1457: flag=_wrap_ModelLoader_setParsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1458: flag=_wrap_ModelLoader_loadModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1459: flag=_wrap_ModelLoader_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1460: flag=_wrap_ModelLoader_loadReducedModelFromFullModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1461: flag=_wrap_ModelLoader_loadReducedModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1462: flag=_wrap_ModelLoader_loadReducedModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1463: flag=_wrap_ModelLoader_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1464: flag=_wrap_ModelLoader_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1465: flag=_wrap_ModelLoader_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1466: flag=_wrap_new_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1467: flag=_wrap_UnknownWrenchContact_unknownType_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1468: flag=_wrap_UnknownWrenchContact_unknownType_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1469: flag=_wrap_UnknownWrenchContact_contactPoint_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1470: flag=_wrap_UnknownWrenchContact_contactPoint_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1471: flag=_wrap_UnknownWrenchContact_forceDirection_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1472: flag=_wrap_UnknownWrenchContact_forceDirection_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1473: flag=_wrap_UnknownWrenchContact_knownWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1474: flag=_wrap_UnknownWrenchContact_knownWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1475: flag=_wrap_UnknownWrenchContact_contactId_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1476: flag=_wrap_UnknownWrenchContact_contactId_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1477: flag=_wrap_delete_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1478: flag=_wrap_new_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1479: flag=_wrap_LinkUnknownWrenchContacts_clear(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1480: flag=_wrap_LinkUnknownWrenchContacts_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1481: flag=_wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1482: flag=_wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1483: flag=_wrap_LinkUnknownWrenchContacts_addNewContactForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1484: flag=_wrap_LinkUnknownWrenchContacts_addNewContactInFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1485: flag=_wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1486: flag=_wrap_LinkUnknownWrenchContacts_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1487: flag=_wrap_LinkUnknownWrenchContacts_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1488: flag=_wrap_delete_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1489: flag=_wrap_new_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1490: flag=_wrap_estimateExternalWrenchesBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1491: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1492: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1493: flag=_wrap_estimateExternalWrenchesBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1494: flag=_wrap_estimateExternalWrenchesBuffers_A_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1495: flag=_wrap_estimateExternalWrenchesBuffers_A_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1496: flag=_wrap_estimateExternalWrenchesBuffers_x_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1497: flag=_wrap_estimateExternalWrenchesBuffers_x_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1498: flag=_wrap_estimateExternalWrenchesBuffers_b_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1499: flag=_wrap_estimateExternalWrenchesBuffers_b_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1500: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1501: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1502: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1503: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1504: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1505: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1506: flag=_wrap_delete_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1507: flag=_wrap_estimateExternalWrenchesWithoutInternalFT(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1508: flag=_wrap_estimateExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1509: flag=_wrap_dynamicsEstimationForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1510: flag=_wrap_dynamicsEstimationForwardVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1511: flag=_wrap_computeLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1512: flag=_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1513: flag=_wrap_new_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1514: flag=_wrap_delete_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1515: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1516: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1517: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1518: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1519: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1520: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_submodels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1521: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1522: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1523: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1524: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1525: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1526: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1527: flag=_wrap_new_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1528: flag=_wrap_delete_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1529: flag=_wrap_SimpleLeggedOdometry_setModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1530: flag=_wrap_SimpleLeggedOdometry_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1531: flag=_wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1532: flag=_wrap_SimpleLeggedOdometry_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1533: flag=_wrap_SimpleLeggedOdometry_updateKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1534: flag=_wrap_SimpleLeggedOdometry_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1535: flag=_wrap_SimpleLeggedOdometry_changeFixedFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1536: flag=_wrap_SimpleLeggedOdometry_getCurrentFixedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1537: flag=_wrap_SimpleLeggedOdometry_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1538: flag=_wrap_isLinkBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1539: flag=_wrap_isJointBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1540: flag=_wrap_isDOFBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1541: flag=_wrap_new_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1542: flag=_wrap_BerdyOptions_berdyVariant_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1543: flag=_wrap_BerdyOptions_berdyVariant_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1544: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1545: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1546: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1547: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1548: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1549: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1550: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1551: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1552: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1553: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1554: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1555: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1556: flag=_wrap_BerdyOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1557: flag=_wrap_BerdyOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1558: flag=_wrap_BerdyOptions_checkConsistency(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1559: flag=_wrap_delete_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1560: flag=_wrap_BerdySensor_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1561: flag=_wrap_BerdySensor_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1562: flag=_wrap_BerdySensor_id_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1563: flag=_wrap_BerdySensor_id_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1564: flag=_wrap_BerdySensor_range_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1565: flag=_wrap_BerdySensor_range_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1566: flag=_wrap_BerdySensor_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1567: flag=_wrap_BerdySensor_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1568: flag=_wrap_new_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1569: flag=_wrap_delete_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1570: flag=_wrap_BerdyDynamicVariable_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1571: flag=_wrap_BerdyDynamicVariable_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1572: flag=_wrap_BerdyDynamicVariable_id_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1573: flag=_wrap_BerdyDynamicVariable_id_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1574: flag=_wrap_BerdyDynamicVariable_range_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1575: flag=_wrap_BerdyDynamicVariable_range_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1576: flag=_wrap_BerdyDynamicVariable_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1577: flag=_wrap_BerdyDynamicVariable_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1578: flag=_wrap_new_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1579: flag=_wrap_delete_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1580: flag=_wrap_new_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1581: flag=_wrap_BerdyHelper_dynamicTraversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1582: flag=_wrap_BerdyHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1583: flag=_wrap_BerdyHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1584: flag=_wrap_BerdyHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1585: flag=_wrap_BerdyHelper_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1586: flag=_wrap_BerdyHelper_getOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1587: flag=_wrap_BerdyHelper_getNrOfDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1588: flag=_wrap_BerdyHelper_getNrOfDynamicEquations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1589: flag=_wrap_BerdyHelper_getNrOfSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1590: flag=_wrap_BerdyHelper_resizeAndZeroBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1591: flag=_wrap_BerdyHelper_getBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1592: flag=_wrap_BerdyHelper_getSensorsOrdering(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1593: flag=_wrap_BerdyHelper_getRangeSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1594: flag=_wrap_BerdyHelper_getRangeDOFSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1595: flag=_wrap_BerdyHelper_getRangeJointSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1596: flag=_wrap_BerdyHelper_getRangeLinkSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1597: flag=_wrap_BerdyHelper_getRangeLinkVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1598: flag=_wrap_BerdyHelper_getRangeJointVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1599: flag=_wrap_BerdyHelper_getRangeDOFVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1600: flag=_wrap_BerdyHelper_getDynamicVariablesOrdering(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1601: flag=_wrap_BerdyHelper_serializeDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1602: flag=_wrap_BerdyHelper_serializeSensorVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1603: flag=_wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1604: flag=_wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1605: flag=_wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1606: flag=_wrap_BerdyHelper_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1607: flag=_wrap_BerdyHelper_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1608: flag=_wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1609: flag=_wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1610: flag=_wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1611: flag=_wrap_delete_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1612: flag=_wrap_new_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1613: flag=_wrap_delete_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1614: flag=_wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1615: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1616: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1617: flag=_wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1618: flag=_wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1619: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1620: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1621: flag=_wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1622: flag=_wrap_BerdySparseMAPSolver_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1623: flag=_wrap_BerdySparseMAPSolver_initialize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1624: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1625: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1626: flag=_wrap_BerdySparseMAPSolver_doEstimate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1627: flag=_wrap_BerdySparseMAPSolver_getLastEstimate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1628: flag=_wrap_delete_IAttitudeEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1629: flag=_wrap_IAttitudeEstimator_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1630: flag=_wrap_IAttitudeEstimator_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1631: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1632: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1633: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1634: flag=_wrap_IAttitudeEstimator_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1635: flag=_wrap_IAttitudeEstimator_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1636: flag=_wrap_IAttitudeEstimator_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1637: flag=_wrap_IAttitudeEstimator_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1638: flag=_wrap_IAttitudeEstimator_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1639: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1640: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1641: flag=_wrap_AttitudeMahonyFilterParameters_kp_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1642: flag=_wrap_AttitudeMahonyFilterParameters_kp_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1643: flag=_wrap_AttitudeMahonyFilterParameters_ki_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1644: flag=_wrap_AttitudeMahonyFilterParameters_ki_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1645: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1646: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1647: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1648: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1649: flag=_wrap_new_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1650: flag=_wrap_delete_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1651: flag=_wrap_new_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1652: flag=_wrap_AttitudeMahonyFilter_useMagnetoMeterMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1653: flag=_wrap_AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1654: flag=_wrap_AttitudeMahonyFilter_setGainkp(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1655: flag=_wrap_AttitudeMahonyFilter_setGainki(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1656: flag=_wrap_AttitudeMahonyFilter_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1657: flag=_wrap_AttitudeMahonyFilter_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1658: flag=_wrap_AttitudeMahonyFilter_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1659: flag=_wrap_AttitudeMahonyFilter_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1660: flag=_wrap_AttitudeMahonyFilter_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1661: flag=_wrap_AttitudeMahonyFilter_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1662: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1663: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1664: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1665: flag=_wrap_AttitudeMahonyFilter_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1666: flag=_wrap_AttitudeMahonyFilter_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1667: flag=_wrap_AttitudeMahonyFilter_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1668: flag=_wrap_AttitudeMahonyFilter_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1669: flag=_wrap_AttitudeMahonyFilter_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1670: flag=_wrap_delete_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1671: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_f(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1672: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1673: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1674: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1675: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfPredict(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1676: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1677: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1678: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1679: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1680: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1681: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1682: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1683: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1684: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1685: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1686: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1687: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1688: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1689: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1690: flag=_wrap_delete_DiscreteExtendedKalmanFilterHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1691: flag=_wrap_output_dimensions_with_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1692: flag=_wrap_output_dimensions_without_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1693: flag=_wrap_input_dimensions_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1694: flag=_wrap_AttitudeEstimatorState_m_orientation_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1695: flag=_wrap_AttitudeEstimatorState_m_orientation_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1696: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1697: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1698: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1699: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1700: flag=_wrap_new_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1701: flag=_wrap_delete_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1702: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1703: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1704: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1705: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1706: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1707: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1708: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1709: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1710: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1711: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1712: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1713: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1714: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1715: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1716: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1717: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1718: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1719: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1720: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1721: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1722: flag=_wrap_new_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1723: flag=_wrap_delete_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1724: flag=_wrap_new_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1725: flag=_wrap_AttitudeQuaternionEKF_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1726: flag=_wrap_AttitudeQuaternionEKF_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1727: flag=_wrap_AttitudeQuaternionEKF_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1728: flag=_wrap_AttitudeQuaternionEKF_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1729: flag=_wrap_AttitudeQuaternionEKF_setBiasCorrelationTimeFactor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1730: flag=_wrap_AttitudeQuaternionEKF_useMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1731: flag=_wrap_AttitudeQuaternionEKF_setMeasurementNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1732: flag=_wrap_AttitudeQuaternionEKF_setSystemNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1733: flag=_wrap_AttitudeQuaternionEKF_setInitialStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1734: flag=_wrap_AttitudeQuaternionEKF_initializeFilter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1735: flag=_wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1736: flag=_wrap_AttitudeQuaternionEKF_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1737: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1738: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1739: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1740: flag=_wrap_AttitudeQuaternionEKF_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1741: flag=_wrap_AttitudeQuaternionEKF_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1742: flag=_wrap_AttitudeQuaternionEKF_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1743: flag=_wrap_AttitudeQuaternionEKF_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1744: flag=_wrap_AttitudeQuaternionEKF_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1745: flag=_wrap_delete_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1746: flag=_wrap_DynamicsRegressorParameter_category_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1747: flag=_wrap_DynamicsRegressorParameter_category_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1748: flag=_wrap_DynamicsRegressorParameter_elemIndex_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1749: flag=_wrap_DynamicsRegressorParameter_elemIndex_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1750: flag=_wrap_DynamicsRegressorParameter_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1751: flag=_wrap_DynamicsRegressorParameter_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1752: flag=_wrap_DynamicsRegressorParameter_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1753: flag=_wrap_DynamicsRegressorParameter_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1754: flag=_wrap_DynamicsRegressorParameter_ne(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1755: flag=_wrap_new_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1756: flag=_wrap_delete_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1757: flag=_wrap_DynamicsRegressorParametersList_parameters_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1758: flag=_wrap_DynamicsRegressorParametersList_parameters_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1759: flag=_wrap_DynamicsRegressorParametersList_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1760: flag=_wrap_DynamicsRegressorParametersList_addParam(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1761: flag=_wrap_DynamicsRegressorParametersList_addList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1762: flag=_wrap_DynamicsRegressorParametersList_findParam(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1763: flag=_wrap_DynamicsRegressorParametersList_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1764: flag=_wrap_new_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1765: flag=_wrap_delete_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1766: flag=_wrap_new_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1767: flag=_wrap_delete_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1768: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1769: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1770: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1771: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1772: flag=_wrap_DynamicsRegressorGenerator_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1773: flag=_wrap_DynamicsRegressorGenerator_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1774: flag=_wrap_DynamicsRegressorGenerator_getNrOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1775: flag=_wrap_DynamicsRegressorGenerator_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1776: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1777: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1778: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutput(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1779: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1780: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1781: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1782: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1783: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1784: flag=_wrap_DynamicsRegressorGenerator_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1785: flag=_wrap_DynamicsRegressorGenerator_getNrOfFakeLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1786: flag=_wrap_DynamicsRegressorGenerator_getBaseLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1787: flag=_wrap_DynamicsRegressorGenerator_getSensorsModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1788: flag=_wrap_DynamicsRegressorGenerator_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1789: flag=_wrap_DynamicsRegressorGenerator_getSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1790: flag=_wrap_DynamicsRegressorGenerator_setTorqueSensorMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1791: flag=_wrap_DynamicsRegressorGenerator_computeRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1792: flag=_wrap_DynamicsRegressorGenerator_getModelParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1793: flag=_wrap_DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1794: flag=_wrap_DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1795: flag=_wrap_DynamicsRegressorGenerator_generate_random_regressors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1796: flag=_wrap_new_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1797: flag=_wrap_delete_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1798: flag=_wrap_KinDynComputations_loadRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1799: flag=_wrap_KinDynComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1800: flag=_wrap_KinDynComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1801: flag=_wrap_KinDynComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1802: flag=_wrap_KinDynComputations_setFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1803: flag=_wrap_KinDynComputations_getFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1804: flag=_wrap_KinDynComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1805: flag=_wrap_KinDynComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1806: flag=_wrap_KinDynComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1807: flag=_wrap_KinDynComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1808: flag=_wrap_KinDynComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1809: flag=_wrap_KinDynComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1810: flag=_wrap_KinDynComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1811: flag=_wrap_KinDynComputations_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1812: flag=_wrap_KinDynComputations_getRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1813: flag=_wrap_KinDynComputations_getRelativeJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1814: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1815: flag=_wrap_KinDynComputations_setJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1816: flag=_wrap_KinDynComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1817: flag=_wrap_KinDynComputations_getRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1818: flag=_wrap_KinDynComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1819: flag=_wrap_KinDynComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1820: flag=_wrap_KinDynComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1821: flag=_wrap_KinDynComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1822: flag=_wrap_KinDynComputations_getModelVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1823: flag=_wrap_KinDynComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1824: flag=_wrap_KinDynComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1825: flag=_wrap_KinDynComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1826: flag=_wrap_KinDynComputations_getRelativeTransformExplicit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1827: flag=_wrap_KinDynComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1828: flag=_wrap_KinDynComputations_getFrameVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1829: flag=_wrap_KinDynComputations_getFrameAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1830: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1831: flag=_wrap_KinDynComputations_getRelativeJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1832: flag=_wrap_KinDynComputations_getRelativeJacobianExplicit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1833: flag=_wrap_KinDynComputations_getFrameBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1834: flag=_wrap_KinDynComputations_getCenterOfMassPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1835: flag=_wrap_KinDynComputations_getCenterOfMassVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1836: flag=_wrap_KinDynComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1837: flag=_wrap_KinDynComputations_getCenterOfMassBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1838: flag=_wrap_KinDynComputations_getAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1839: flag=_wrap_KinDynComputations_getAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1840: flag=_wrap_KinDynComputations_getCentroidalAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1841: flag=_wrap_KinDynComputations_getCentroidalAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1842: flag=_wrap_KinDynComputations_getLinearAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1843: flag=_wrap_KinDynComputations_getLinearAngularMomentumJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1844: flag=_wrap_KinDynComputations_getCentroidalTotalMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1845: flag=_wrap_KinDynComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1846: flag=_wrap_KinDynComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1847: flag=_wrap_KinDynComputations_generalizedBiasForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1848: flag=_wrap_KinDynComputations_generalizedGravityForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1849: flag=_wrap_KinDynComputations_generalizedExternalForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1850: flag=_wrap_KinDynComputations_inverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1851: flag=_wrap_delete_ICamera(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1852: flag=_wrap_ICamera_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1853: flag=_wrap_ICamera_setTarget(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1854: flag=_wrap_ICamera_setUpVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1855: flag=_wrap_ColorViz_r_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1856: flag=_wrap_ColorViz_r_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1857: flag=_wrap_ColorViz_g_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1858: flag=_wrap_ColorViz_g_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1859: flag=_wrap_ColorViz_b_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1860: flag=_wrap_ColorViz_b_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1861: flag=_wrap_ColorViz_a_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1862: flag=_wrap_ColorViz_a_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1863: flag=_wrap_new_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1864: flag=_wrap_delete_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1865: flag=_wrap_delete_ILight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1866: flag=_wrap_ILight_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1867: flag=_wrap_ILight_setType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1868: flag=_wrap_ILight_getType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1869: flag=_wrap_ILight_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1870: flag=_wrap_ILight_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1871: flag=_wrap_ILight_setDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1872: flag=_wrap_ILight_getDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1873: flag=_wrap_ILight_setAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1874: flag=_wrap_ILight_getAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1875: flag=_wrap_ILight_setSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1876: flag=_wrap_ILight_getSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1877: flag=_wrap_ILight_setDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1878: flag=_wrap_ILight_getDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1879: flag=_wrap_delete_IEnvironment(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1880: flag=_wrap_IEnvironment_getElements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1881: flag=_wrap_IEnvironment_setElementVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1882: flag=_wrap_IEnvironment_setBackgroundColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1883: flag=_wrap_IEnvironment_setAmbientLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1884: flag=_wrap_IEnvironment_getLights(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1885: flag=_wrap_IEnvironment_addLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1886: flag=_wrap_IEnvironment_lightViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1887: flag=_wrap_IEnvironment_removeLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1888: flag=_wrap_delete_IJetsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1889: flag=_wrap_IJetsVisualization_setJetsFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1890: flag=_wrap_IJetsVisualization_getNrOfJets(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1891: flag=_wrap_IJetsVisualization_getJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1892: flag=_wrap_IJetsVisualization_setJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1893: flag=_wrap_IJetsVisualization_setJetColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1894: flag=_wrap_IJetsVisualization_setJetsDimensions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1895: flag=_wrap_IJetsVisualization_setJetsIntensity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1896: flag=_wrap_delete_IVectorsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1897: flag=_wrap_IVectorsVisualization_addVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1898: flag=_wrap_IVectorsVisualization_getNrOfVectors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1899: flag=_wrap_IVectorsVisualization_getVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1900: flag=_wrap_IVectorsVisualization_updateVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1901: flag=_wrap_IVectorsVisualization_setVectorColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1902: flag=_wrap_IVectorsVisualization_setVectorsAspect(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1903: flag=_wrap_delete_IModelVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1904: flag=_wrap_IModelVisualization_setPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1905: flag=_wrap_IModelVisualization_setLinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1906: flag=_wrap_IModelVisualization_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1907: flag=_wrap_IModelVisualization_getInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1908: flag=_wrap_IModelVisualization_setModelVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1909: flag=_wrap_IModelVisualization_setModelColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1910: flag=_wrap_IModelVisualization_resetModelColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1911: flag=_wrap_IModelVisualization_setLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1912: flag=_wrap_IModelVisualization_resetLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1913: flag=_wrap_IModelVisualization_getLinkNames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1914: flag=_wrap_IModelVisualization_setLinkVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1915: flag=_wrap_IModelVisualization_getFeatures(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1916: flag=_wrap_IModelVisualization_setFeatureVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1917: flag=_wrap_IModelVisualization_jets(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1918: flag=_wrap_IModelVisualization_getWorldModelTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1919: flag=_wrap_IModelVisualization_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1920: flag=_wrap_VisualizerOptions_verbose_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1921: flag=_wrap_VisualizerOptions_verbose_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1922: flag=_wrap_VisualizerOptions_winWidth_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1923: flag=_wrap_VisualizerOptions_winWidth_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1924: flag=_wrap_VisualizerOptions_winHeight_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1925: flag=_wrap_VisualizerOptions_winHeight_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1926: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1927: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1928: flag=_wrap_new_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1929: flag=_wrap_delete_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1930: flag=_wrap_new_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1931: flag=_wrap_delete_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1932: flag=_wrap_Visualizer_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1933: flag=_wrap_Visualizer_getNrOfVisualizedModels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1934: flag=_wrap_Visualizer_getModelInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1935: flag=_wrap_Visualizer_getModelInstanceIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1936: flag=_wrap_Visualizer_addModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1937: flag=_wrap_Visualizer_modelViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1938: flag=_wrap_Visualizer_camera(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1939: flag=_wrap_Visualizer_enviroment(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1940: flag=_wrap_Visualizer_vectors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1941: flag=_wrap_Visualizer_run(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1942: flag=_wrap_Visualizer_draw(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1943: flag=_wrap_Visualizer_drawToFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1944: flag=_wrap_Visualizer_close(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1945: flag=_wrap_new_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1946: flag=_wrap_delete_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1947: flag=_wrap_DynamicsComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1948: flag=_wrap_DynamicsComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1949: flag=_wrap_DynamicsComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1950: flag=_wrap_DynamicsComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1951: flag=_wrap_DynamicsComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1952: flag=_wrap_DynamicsComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1953: flag=_wrap_DynamicsComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1954: flag=_wrap_DynamicsComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1955: flag=_wrap_DynamicsComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1956: flag=_wrap_DynamicsComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1957: flag=_wrap_DynamicsComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1958: flag=_wrap_DynamicsComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1959: flag=_wrap_DynamicsComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1960: flag=_wrap_DynamicsComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1961: flag=_wrap_DynamicsComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1962: flag=_wrap_DynamicsComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1963: flag=_wrap_DynamicsComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1964: flag=_wrap_DynamicsComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1965: flag=_wrap_DynamicsComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1966: flag=_wrap_DynamicsComputations_getFrameTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1967: flag=_wrap_DynamicsComputations_getFrameTwistInWorldOrient(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1968: flag=_wrap_DynamicsComputations_getFrameProperSpatialAcceleration(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1969: flag=_wrap_DynamicsComputations_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1970: flag=_wrap_DynamicsComputations_getLinkInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1971: flag=_wrap_DynamicsComputations_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1972: flag=_wrap_DynamicsComputations_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1973: flag=_wrap_DynamicsComputations_getJointLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1974: flag=_wrap_DynamicsComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1975: flag=_wrap_DynamicsComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1976: flag=_wrap_DynamicsComputations_getFrameJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1977: flag=_wrap_DynamicsComputations_getDynamicsRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1978: flag=_wrap_DynamicsComputations_getModelDynamicsParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1979: flag=_wrap_DynamicsComputations_getCenterOfMass(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1980: flag=_wrap_DynamicsComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 768: flag=_wrap_Rotation_RPYRightTrivializedDerivativeRateOfChange(resc,resv,argc,(mxArray**)(argv)); break;
+  case 769: flag=_wrap_Rotation_RPYRightTrivializedDerivativeInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 770: flag=_wrap_Rotation_RPYRightTrivializedDerivativeInverseRateOfChange(resc,resv,argc,(mxArray**)(argv)); break;
+  case 771: flag=_wrap_Rotation_QuaternionRightTrivializedDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 772: flag=_wrap_Rotation_QuaternionRightTrivializedDerivativeInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 773: flag=_wrap_Rotation_Identity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 774: flag=_wrap_Rotation_RotationFromQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 775: flag=_wrap_Rotation_leftJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 776: flag=_wrap_Rotation_leftJacobianInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 777: flag=_wrap_Rotation_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 778: flag=_wrap_Rotation_display(resc,resv,argc,(mxArray**)(argv)); break;
+  case 779: flag=_wrap_delete_Rotation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 780: flag=_wrap_new_TransformSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 781: flag=_wrap_TransformSemantics_getRotationSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 782: flag=_wrap_TransformSemantics_getPositionSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 783: flag=_wrap_TransformSemantics_setRotationSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 784: flag=_wrap_TransformSemantics_setPositionSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 785: flag=_wrap_TransformSemantics_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 786: flag=_wrap_TransformSemantics_display(resc,resv,argc,(mxArray**)(argv)); break;
+  case 787: flag=_wrap_delete_TransformSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 788: flag=_wrap_new_Transform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 789: flag=_wrap_Transform_fromHomogeneousTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 790: flag=_wrap_Transform_getSemantics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 791: flag=_wrap_Transform_getRotation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 792: flag=_wrap_Transform_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 793: flag=_wrap_Transform_setRotation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 794: flag=_wrap_Transform_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 795: flag=_wrap_Transform_compose(resc,resv,argc,(mxArray**)(argv)); break;
+  case 796: flag=_wrap_Transform_inverse2(resc,resv,argc,(mxArray**)(argv)); break;
+  case 797: flag=_wrap_Transform_inverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 798: flag=_wrap_Transform_mtimes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 799: flag=_wrap_Transform_Identity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 800: flag=_wrap_Transform_asHomogeneousTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 801: flag=_wrap_Transform_asAdjointTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 802: flag=_wrap_Transform_asAdjointTransformWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 803: flag=_wrap_Transform_log(resc,resv,argc,(mxArray**)(argv)); break;
+  case 804: flag=_wrap_Transform_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 805: flag=_wrap_Transform_display(resc,resv,argc,(mxArray**)(argv)); break;
+  case 806: flag=_wrap_delete_Transform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 807: flag=_wrap_new_TransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 808: flag=_wrap_delete_TransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 809: flag=_wrap_TransformDerivative_getRotationDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 810: flag=_wrap_TransformDerivative_getPositionDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 811: flag=_wrap_TransformDerivative_setRotationDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 812: flag=_wrap_TransformDerivative_setPositionDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 813: flag=_wrap_TransformDerivative_Zero(resc,resv,argc,(mxArray**)(argv)); break;
+  case 814: flag=_wrap_TransformDerivative_asHomogeneousTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 815: flag=_wrap_TransformDerivative_asAdjointTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 816: flag=_wrap_TransformDerivative_asAdjointTransformWrenchDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 817: flag=_wrap_TransformDerivative_mtimes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 818: flag=_wrap_TransformDerivative_derivativeOfInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 819: flag=_wrap_TransformDerivative_transform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 820: flag=_wrap_dynamic_extent_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 821: flag=_wrap_DynamicSpan_extent_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 822: flag=_wrap_new_DynamicSpan(resc,resv,argc,(mxArray**)(argv)); break;
+  case 823: flag=_wrap_delete_DynamicSpan(resc,resv,argc,(mxArray**)(argv)); break;
+  case 824: flag=_wrap_DynamicSpan_first(resc,resv,argc,(mxArray**)(argv)); break;
+  case 825: flag=_wrap_DynamicSpan_last(resc,resv,argc,(mxArray**)(argv)); break;
+  case 826: flag=_wrap_DynamicSpan_subspan(resc,resv,argc,(mxArray**)(argv)); break;
+  case 827: flag=_wrap_DynamicSpan_size(resc,resv,argc,(mxArray**)(argv)); break;
+  case 828: flag=_wrap_DynamicSpan_size_bytes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 829: flag=_wrap_DynamicSpan_empty(resc,resv,argc,(mxArray**)(argv)); break;
+  case 830: flag=_wrap_DynamicSpan_brace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 831: flag=_wrap_DynamicSpan_getVal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 832: flag=_wrap_DynamicSpan_setVal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 833: flag=_wrap_DynamicSpan_at(resc,resv,argc,(mxArray**)(argv)); break;
+  case 834: flag=_wrap_DynamicSpan_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 835: flag=_wrap_DynamicSpan_begin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 836: flag=_wrap_DynamicSpan_end(resc,resv,argc,(mxArray**)(argv)); break;
+  case 837: flag=_wrap_DynamicSpan_cbegin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 838: flag=_wrap_DynamicSpan_cend(resc,resv,argc,(mxArray**)(argv)); break;
+  case 839: flag=_wrap_DynamicSpan_rbegin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 840: flag=_wrap_DynamicSpan_rend(resc,resv,argc,(mxArray**)(argv)); break;
+  case 841: flag=_wrap_DynamicSpan_crbegin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 842: flag=_wrap_DynamicSpan_crend(resc,resv,argc,(mxArray**)(argv)); break;
+  case 843: flag=_wrap_LINK_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 844: flag=_wrap_LINK_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 845: flag=_wrap_LINK_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 846: flag=_wrap_LINK_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 847: flag=_wrap_JOINT_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 848: flag=_wrap_JOINT_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 849: flag=_wrap_JOINT_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 850: flag=_wrap_JOINT_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 851: flag=_wrap_DOF_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 852: flag=_wrap_DOF_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 853: flag=_wrap_DOF_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 854: flag=_wrap_DOF_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 855: flag=_wrap_FRAME_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 856: flag=_wrap_FRAME_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 857: flag=_wrap_FRAME_INVALID_NAME_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 858: flag=_wrap_FRAME_INVALID_NAME_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 859: flag=_wrap_TRAVERSAL_INVALID_INDEX_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 860: flag=_wrap_TRAVERSAL_INVALID_INDEX_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 861: flag=_wrap_new_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 862: flag=_wrap_LinkPositions_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 863: flag=_wrap_LinkPositions_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 864: flag=_wrap_LinkPositions_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 865: flag=_wrap_LinkPositions_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 866: flag=_wrap_LinkPositions_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 867: flag=_wrap_delete_LinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 868: flag=_wrap_new_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 869: flag=_wrap_LinkWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 870: flag=_wrap_LinkWrenches_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 871: flag=_wrap_LinkWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 872: flag=_wrap_LinkWrenches_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 873: flag=_wrap_LinkWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 874: flag=_wrap_LinkWrenches_zero(resc,resv,argc,(mxArray**)(argv)); break;
+  case 875: flag=_wrap_delete_LinkWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 876: flag=_wrap_new_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 877: flag=_wrap_LinkInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 878: flag=_wrap_LinkInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 879: flag=_wrap_LinkInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 880: flag=_wrap_delete_LinkInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 881: flag=_wrap_new_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 882: flag=_wrap_LinkArticulatedBodyInertias_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 883: flag=_wrap_LinkArticulatedBodyInertias_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 884: flag=_wrap_LinkArticulatedBodyInertias_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 885: flag=_wrap_delete_LinkArticulatedBodyInertias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 886: flag=_wrap_new_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 887: flag=_wrap_LinkVelArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 888: flag=_wrap_LinkVelArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 889: flag=_wrap_LinkVelArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 890: flag=_wrap_LinkVelArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 891: flag=_wrap_LinkVelArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 892: flag=_wrap_delete_LinkVelArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 893: flag=_wrap_new_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 894: flag=_wrap_LinkAccArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 895: flag=_wrap_LinkAccArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 896: flag=_wrap_LinkAccArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 897: flag=_wrap_LinkAccArray_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 898: flag=_wrap_LinkAccArray_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 899: flag=_wrap_delete_LinkAccArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 900: flag=_wrap_new_Link(resc,resv,argc,(mxArray**)(argv)); break;
+  case 901: flag=_wrap_Link_inertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 902: flag=_wrap_Link_setInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 903: flag=_wrap_Link_getInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 904: flag=_wrap_Link_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 905: flag=_wrap_Link_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 906: flag=_wrap_delete_Link(resc,resv,argc,(mxArray**)(argv)); break;
+  case 907: flag=_wrap_delete_IJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 908: flag=_wrap_IJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 909: flag=_wrap_IJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 910: flag=_wrap_IJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 911: flag=_wrap_IJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 912: flag=_wrap_IJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 913: flag=_wrap_IJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 914: flag=_wrap_IJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 915: flag=_wrap_IJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 916: flag=_wrap_IJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 917: flag=_wrap_IJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 918: flag=_wrap_IJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 919: flag=_wrap_IJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 920: flag=_wrap_IJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 921: flag=_wrap_IJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 922: flag=_wrap_IJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 923: flag=_wrap_IJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 924: flag=_wrap_IJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 925: flag=_wrap_IJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 926: flag=_wrap_IJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 927: flag=_wrap_IJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 928: flag=_wrap_IJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 929: flag=_wrap_IJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 930: flag=_wrap_IJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 931: flag=_wrap_IJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 932: flag=_wrap_IJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 933: flag=_wrap_IJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 934: flag=_wrap_IJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 935: flag=_wrap_IJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 936: flag=_wrap_IJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 937: flag=_wrap_IJoint_isRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 938: flag=_wrap_IJoint_isFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 939: flag=_wrap_IJoint_asRevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 940: flag=_wrap_IJoint_asFixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 941: flag=_wrap_new_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 942: flag=_wrap_delete_FixedJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 943: flag=_wrap_FixedJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 944: flag=_wrap_FixedJoint_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 945: flag=_wrap_FixedJoint_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 946: flag=_wrap_FixedJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 947: flag=_wrap_FixedJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 948: flag=_wrap_FixedJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 949: flag=_wrap_FixedJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 950: flag=_wrap_FixedJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 951: flag=_wrap_FixedJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 952: flag=_wrap_FixedJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 953: flag=_wrap_FixedJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 954: flag=_wrap_FixedJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 955: flag=_wrap_FixedJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 956: flag=_wrap_FixedJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 957: flag=_wrap_FixedJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 958: flag=_wrap_FixedJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 959: flag=_wrap_FixedJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 960: flag=_wrap_FixedJoint_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 961: flag=_wrap_FixedJoint_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 962: flag=_wrap_FixedJoint_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 963: flag=_wrap_FixedJoint_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 964: flag=_wrap_FixedJoint_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 965: flag=_wrap_FixedJoint_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 966: flag=_wrap_FixedJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 967: flag=_wrap_FixedJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 968: flag=_wrap_FixedJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 969: flag=_wrap_FixedJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 970: flag=_wrap_FixedJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 971: flag=_wrap_FixedJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 972: flag=_wrap_delete_MovableJointImpl1(resc,resv,argc,(mxArray**)(argv)); break;
+  case 973: flag=_wrap_MovableJointImpl1_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 974: flag=_wrap_MovableJointImpl1_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 975: flag=_wrap_MovableJointImpl1_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 976: flag=_wrap_MovableJointImpl1_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 977: flag=_wrap_MovableJointImpl1_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 978: flag=_wrap_MovableJointImpl1_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 979: flag=_wrap_MovableJointImpl1_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 980: flag=_wrap_MovableJointImpl1_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 981: flag=_wrap_delete_MovableJointImpl2(resc,resv,argc,(mxArray**)(argv)); break;
+  case 982: flag=_wrap_MovableJointImpl2_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 983: flag=_wrap_MovableJointImpl2_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 984: flag=_wrap_MovableJointImpl2_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 985: flag=_wrap_MovableJointImpl2_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 986: flag=_wrap_MovableJointImpl2_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 987: flag=_wrap_MovableJointImpl2_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 988: flag=_wrap_MovableJointImpl2_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 989: flag=_wrap_MovableJointImpl2_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 990: flag=_wrap_delete_MovableJointImpl3(resc,resv,argc,(mxArray**)(argv)); break;
+  case 991: flag=_wrap_MovableJointImpl3_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 992: flag=_wrap_MovableJointImpl3_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 993: flag=_wrap_MovableJointImpl3_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 994: flag=_wrap_MovableJointImpl3_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 995: flag=_wrap_MovableJointImpl3_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 996: flag=_wrap_MovableJointImpl3_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 997: flag=_wrap_MovableJointImpl3_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 998: flag=_wrap_MovableJointImpl3_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 999: flag=_wrap_delete_MovableJointImpl4(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1000: flag=_wrap_MovableJointImpl4_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1001: flag=_wrap_MovableJointImpl4_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1002: flag=_wrap_MovableJointImpl4_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1003: flag=_wrap_MovableJointImpl4_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1004: flag=_wrap_MovableJointImpl4_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1005: flag=_wrap_MovableJointImpl4_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1006: flag=_wrap_MovableJointImpl4_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1007: flag=_wrap_MovableJointImpl4_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1008: flag=_wrap_delete_MovableJointImpl5(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1009: flag=_wrap_MovableJointImpl5_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1010: flag=_wrap_MovableJointImpl5_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1011: flag=_wrap_MovableJointImpl5_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1012: flag=_wrap_MovableJointImpl5_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1013: flag=_wrap_MovableJointImpl5_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1014: flag=_wrap_MovableJointImpl5_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1015: flag=_wrap_MovableJointImpl5_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1016: flag=_wrap_MovableJointImpl5_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1017: flag=_wrap_delete_MovableJointImpl6(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1018: flag=_wrap_MovableJointImpl6_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1019: flag=_wrap_MovableJointImpl6_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1020: flag=_wrap_MovableJointImpl6_setIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1021: flag=_wrap_MovableJointImpl6_getIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1022: flag=_wrap_MovableJointImpl6_setPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1023: flag=_wrap_MovableJointImpl6_getPosCoordsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1024: flag=_wrap_MovableJointImpl6_setDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1025: flag=_wrap_MovableJointImpl6_getDOFsOffset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1026: flag=_wrap_new_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1027: flag=_wrap_delete_RevoluteJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1028: flag=_wrap_RevoluteJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1029: flag=_wrap_RevoluteJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1030: flag=_wrap_RevoluteJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1031: flag=_wrap_RevoluteJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1032: flag=_wrap_RevoluteJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1033: flag=_wrap_RevoluteJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1034: flag=_wrap_RevoluteJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1035: flag=_wrap_RevoluteJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1036: flag=_wrap_RevoluteJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1037: flag=_wrap_RevoluteJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1038: flag=_wrap_RevoluteJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1039: flag=_wrap_RevoluteJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1040: flag=_wrap_RevoluteJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1041: flag=_wrap_RevoluteJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1042: flag=_wrap_RevoluteJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1043: flag=_wrap_RevoluteJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1044: flag=_wrap_RevoluteJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1045: flag=_wrap_RevoluteJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1046: flag=_wrap_RevoluteJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1047: flag=_wrap_RevoluteJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1048: flag=_wrap_RevoluteJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1049: flag=_wrap_RevoluteJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1050: flag=_wrap_RevoluteJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1051: flag=_wrap_new_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1052: flag=_wrap_delete_PrismaticJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1053: flag=_wrap_PrismaticJoint_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1054: flag=_wrap_PrismaticJoint_setAttachedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1055: flag=_wrap_PrismaticJoint_setRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1056: flag=_wrap_PrismaticJoint_setAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1057: flag=_wrap_PrismaticJoint_getFirstAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1058: flag=_wrap_PrismaticJoint_getSecondAttachedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1059: flag=_wrap_PrismaticJoint_getAxis(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1060: flag=_wrap_PrismaticJoint_getRestTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1061: flag=_wrap_PrismaticJoint_getTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1062: flag=_wrap_PrismaticJoint_getTransformDerivative(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1063: flag=_wrap_PrismaticJoint_getMotionSubspaceVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1064: flag=_wrap_PrismaticJoint_computeChildPosVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1065: flag=_wrap_PrismaticJoint_computeChildVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1066: flag=_wrap_PrismaticJoint_computeChildVelAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1067: flag=_wrap_PrismaticJoint_computeChildAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1068: flag=_wrap_PrismaticJoint_computeChildBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1069: flag=_wrap_PrismaticJoint_computeJointTorque(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1070: flag=_wrap_PrismaticJoint_hasPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1071: flag=_wrap_PrismaticJoint_enablePosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1072: flag=_wrap_PrismaticJoint_getPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1073: flag=_wrap_PrismaticJoint_getMinPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1074: flag=_wrap_PrismaticJoint_getMaxPosLimit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1075: flag=_wrap_PrismaticJoint_setPosLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1076: flag=_wrap_new_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1077: flag=_wrap_delete_Traversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1078: flag=_wrap_Traversal_getNrOfVisitedLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1079: flag=_wrap_Traversal_getLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1080: flag=_wrap_Traversal_getBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1081: flag=_wrap_Traversal_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1082: flag=_wrap_Traversal_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1083: flag=_wrap_Traversal_getParentLinkFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1084: flag=_wrap_Traversal_getParentJointFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1085: flag=_wrap_Traversal_getTraversalIndexFromLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1086: flag=_wrap_Traversal_reset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1087: flag=_wrap_Traversal_addTraversalBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1088: flag=_wrap_Traversal_addTraversalElement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1089: flag=_wrap_Traversal_isParentOf(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1090: flag=_wrap_Traversal_getChildLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1091: flag=_wrap_Traversal_getParentLinkIndexFromJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1092: flag=_wrap_Traversal_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1093: flag=_wrap_delete_SolidShape(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1094: flag=_wrap_SolidShape_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1095: flag=_wrap_SolidShape_name_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1096: flag=_wrap_SolidShape_name_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1097: flag=_wrap_SolidShape_nameIsValid_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1098: flag=_wrap_SolidShape_nameIsValid_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1099: flag=_wrap_SolidShape_link_H_geometry_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1100: flag=_wrap_SolidShape_link_H_geometry_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1101: flag=_wrap_SolidShape_material_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1102: flag=_wrap_SolidShape_material_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1103: flag=_wrap_SolidShape_isSphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1104: flag=_wrap_SolidShape_isBox(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1105: flag=_wrap_SolidShape_isCylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1106: flag=_wrap_SolidShape_isExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1107: flag=_wrap_SolidShape_asSphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1108: flag=_wrap_SolidShape_asBox(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1109: flag=_wrap_SolidShape_asCylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1110: flag=_wrap_SolidShape_asExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1111: flag=_wrap_delete_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1112: flag=_wrap_Sphere_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1113: flag=_wrap_Sphere_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1114: flag=_wrap_Sphere_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1115: flag=_wrap_new_Sphere(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1116: flag=_wrap_delete_Box(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1117: flag=_wrap_Box_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1118: flag=_wrap_Box_x_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1119: flag=_wrap_Box_x_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1120: flag=_wrap_Box_y_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1121: flag=_wrap_Box_y_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1122: flag=_wrap_Box_z_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1123: flag=_wrap_Box_z_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1124: flag=_wrap_new_Box(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1125: flag=_wrap_delete_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1126: flag=_wrap_Cylinder_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1127: flag=_wrap_Cylinder_length_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1128: flag=_wrap_Cylinder_length_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1129: flag=_wrap_Cylinder_radius_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1130: flag=_wrap_Cylinder_radius_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1131: flag=_wrap_new_Cylinder(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1132: flag=_wrap_delete_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1133: flag=_wrap_ExternalMesh_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1134: flag=_wrap_ExternalMesh_filename_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1135: flag=_wrap_ExternalMesh_filename_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1136: flag=_wrap_ExternalMesh_scale_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1137: flag=_wrap_ExternalMesh_scale_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1138: flag=_wrap_new_ExternalMesh(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1139: flag=_wrap_new_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1140: flag=_wrap_ModelSolidShapes_clear(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1141: flag=_wrap_delete_ModelSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1142: flag=_wrap_ModelSolidShapes_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1143: flag=_wrap_ModelSolidShapes_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1144: flag=_wrap_ModelSolidShapes_linkSolidShapes_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1145: flag=_wrap_ModelSolidShapes_linkSolidShapes_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1146: flag=_wrap_Neighbor_neighborLink_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1147: flag=_wrap_Neighbor_neighborLink_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1148: flag=_wrap_Neighbor_neighborJoint_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1149: flag=_wrap_Neighbor_neighborJoint_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1150: flag=_wrap_new_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1151: flag=_wrap_delete_Neighbor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1152: flag=_wrap_new_Model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1153: flag=_wrap_Model_copy(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1154: flag=_wrap_delete_Model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1155: flag=_wrap_Model_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1156: flag=_wrap_Model_getLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1157: flag=_wrap_Model_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1158: flag=_wrap_Model_isValidLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1159: flag=_wrap_Model_getLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1160: flag=_wrap_Model_addLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1161: flag=_wrap_Model_getNrOfJoints(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1162: flag=_wrap_Model_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1163: flag=_wrap_Model_getTotalMass(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1164: flag=_wrap_Model_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1165: flag=_wrap_Model_getJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1166: flag=_wrap_Model_isValidJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1167: flag=_wrap_Model_isLinkNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1168: flag=_wrap_Model_isJointNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1169: flag=_wrap_Model_isFrameNameUsed(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1170: flag=_wrap_Model_addJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1171: flag=_wrap_Model_addJointAndLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1172: flag=_wrap_Model_insertLinkToExistingJointAndAddJointForDisplacedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1173: flag=_wrap_Model_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1174: flag=_wrap_Model_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1175: flag=_wrap_Model_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1176: flag=_wrap_Model_addAdditionalFrameToLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1177: flag=_wrap_Model_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1178: flag=_wrap_Model_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1179: flag=_wrap_Model_isValidFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1180: flag=_wrap_Model_getFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1181: flag=_wrap_Model_getFrameLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1182: flag=_wrap_Model_getLinkAdditionalFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1183: flag=_wrap_Model_getNrOfNeighbors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1184: flag=_wrap_Model_getNeighbor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1185: flag=_wrap_Model_setDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1186: flag=_wrap_Model_getDefaultBaseLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1187: flag=_wrap_Model_computeFullTreeTraversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1188: flag=_wrap_Model_getInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1189: flag=_wrap_Model_updateInertialParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1190: flag=_wrap_Model_visualSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1191: flag=_wrap_Model_collisionSolidShapes(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1192: flag=_wrap_Model_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1193: flag=_wrap_new_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1194: flag=_wrap_JointPosDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1195: flag=_wrap_JointPosDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1196: flag=_wrap_delete_JointPosDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1197: flag=_wrap_new_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1198: flag=_wrap_JointDOFsDoubleArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1199: flag=_wrap_JointDOFsDoubleArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1200: flag=_wrap_delete_JointDOFsDoubleArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1201: flag=_wrap_new_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1202: flag=_wrap_DOFSpatialForceArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1203: flag=_wrap_DOFSpatialForceArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1204: flag=_wrap_DOFSpatialForceArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1205: flag=_wrap_delete_DOFSpatialForceArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1206: flag=_wrap_new_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1207: flag=_wrap_DOFSpatialMotionArray_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1208: flag=_wrap_DOFSpatialMotionArray_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1209: flag=_wrap_DOFSpatialMotionArray_paren(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1210: flag=_wrap_delete_DOFSpatialMotionArray(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1211: flag=_wrap_new_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1212: flag=_wrap_FrameFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1213: flag=_wrap_FrameFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1214: flag=_wrap_delete_FrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1215: flag=_wrap_new_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1216: flag=_wrap_MomentumFreeFloatingJacobian_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1217: flag=_wrap_MomentumFreeFloatingJacobian_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1218: flag=_wrap_delete_MomentumFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1219: flag=_wrap_new_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1220: flag=_wrap_FreeFloatingMassMatrix_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1221: flag=_wrap_delete_FreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1222: flag=_wrap_new_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1223: flag=_wrap_FreeFloatingPos_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1224: flag=_wrap_FreeFloatingPos_worldBasePos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1225: flag=_wrap_FreeFloatingPos_jointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1226: flag=_wrap_FreeFloatingPos_getNrOfPosCoords(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1227: flag=_wrap_delete_FreeFloatingPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1228: flag=_wrap_new_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1229: flag=_wrap_FreeFloatingGeneralizedTorques_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1230: flag=_wrap_FreeFloatingGeneralizedTorques_baseWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1231: flag=_wrap_FreeFloatingGeneralizedTorques_jointTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1232: flag=_wrap_FreeFloatingGeneralizedTorques_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1233: flag=_wrap_delete_FreeFloatingGeneralizedTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1234: flag=_wrap_new_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1235: flag=_wrap_FreeFloatingVel_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1236: flag=_wrap_FreeFloatingVel_baseVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1237: flag=_wrap_FreeFloatingVel_jointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1238: flag=_wrap_FreeFloatingVel_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1239: flag=_wrap_delete_FreeFloatingVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1240: flag=_wrap_new_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1241: flag=_wrap_FreeFloatingAcc_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1242: flag=_wrap_FreeFloatingAcc_baseAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1243: flag=_wrap_FreeFloatingAcc_jointAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1244: flag=_wrap_FreeFloatingAcc_getNrOfDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1245: flag=_wrap_delete_FreeFloatingAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1246: flag=_wrap_ContactWrench_contactId(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1247: flag=_wrap_ContactWrench_contactPoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1248: flag=_wrap_ContactWrench_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1249: flag=_wrap_new_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1250: flag=_wrap_delete_ContactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1251: flag=_wrap_new_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1252: flag=_wrap_LinkContactWrenches_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1253: flag=_wrap_LinkContactWrenches_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1254: flag=_wrap_LinkContactWrenches_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1255: flag=_wrap_LinkContactWrenches_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1256: flag=_wrap_LinkContactWrenches_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1257: flag=_wrap_LinkContactWrenches_computeNetWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1258: flag=_wrap_LinkContactWrenches_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1259: flag=_wrap_delete_LinkContactWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1260: flag=_wrap_ForwardPositionKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1261: flag=_wrap_ForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1262: flag=_wrap_ForwardPosVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1263: flag=_wrap_ForwardPosVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1264: flag=_wrap_ForwardAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1265: flag=_wrap_ForwardBiasAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1266: flag=_wrap_ComputeLinearAndAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1267: flag=_wrap_ComputeLinearAndAngularMomentumDerivativeBias(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1268: flag=_wrap_RNEADynamicPhase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1269: flag=_wrap_CompositeRigidBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1270: flag=_wrap_new_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1271: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1272: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1273: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1274: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_S_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1275: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1276: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_U_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1277: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1278: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_D_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1279: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1280: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_u_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1281: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1282: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksVel_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1283: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1284: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasAcceleration_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1285: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1286: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksAccelerations_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1287: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1288: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linkABIs_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1289: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1290: flag=_wrap_ArticulatedBodyAlgorithmInternalBuffers_linksBiasWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1291: flag=_wrap_delete_ArticulatedBodyAlgorithmInternalBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1292: flag=_wrap_ArticulatedBodyAlgorithm(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1293: flag=_wrap_InverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1294: flag=_wrap_NR_OF_SENSOR_TYPES_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1295: flag=_wrap_isLinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1296: flag=_wrap_isJointSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1297: flag=_wrap_getSensorTypeSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1298: flag=_wrap_delete_Sensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1299: flag=_wrap_Sensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1300: flag=_wrap_Sensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1301: flag=_wrap_Sensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1302: flag=_wrap_Sensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1303: flag=_wrap_Sensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1304: flag=_wrap_Sensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1305: flag=_wrap_Sensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1306: flag=_wrap_Sensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1307: flag=_wrap_delete_JointSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1308: flag=_wrap_JointSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1309: flag=_wrap_JointSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1310: flag=_wrap_JointSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1311: flag=_wrap_JointSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1312: flag=_wrap_JointSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1313: flag=_wrap_delete_LinkSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1314: flag=_wrap_LinkSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1315: flag=_wrap_LinkSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1316: flag=_wrap_LinkSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1317: flag=_wrap_LinkSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1318: flag=_wrap_LinkSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1319: flag=_wrap_LinkSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1320: flag=_wrap_LinkSensor_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1321: flag=_wrap_new_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1322: flag=_wrap_delete_SensorsList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1323: flag=_wrap_SensorsList_addSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1324: flag=_wrap_SensorsList_setSerialization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1325: flag=_wrap_SensorsList_getSerialization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1326: flag=_wrap_SensorsList_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1327: flag=_wrap_SensorsList_getSensorIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1328: flag=_wrap_SensorsList_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1329: flag=_wrap_SensorsList_getSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1330: flag=_wrap_SensorsList_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1331: flag=_wrap_SensorsList_removeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1332: flag=_wrap_SensorsList_removeAllSensorsOfType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1333: flag=_wrap_SensorsList_getSixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1334: flag=_wrap_SensorsList_getAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1335: flag=_wrap_SensorsList_getGyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1336: flag=_wrap_SensorsList_getThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1337: flag=_wrap_SensorsList_getThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1338: flag=_wrap_new_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1339: flag=_wrap_delete_SensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1340: flag=_wrap_SensorsMeasurements_setNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1341: flag=_wrap_SensorsMeasurements_getNrOfSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1342: flag=_wrap_SensorsMeasurements_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1343: flag=_wrap_SensorsMeasurements_toVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1344: flag=_wrap_SensorsMeasurements_setMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1345: flag=_wrap_SensorsMeasurements_getMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1346: flag=_wrap_SensorsMeasurements_getSizeOfAllSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1347: flag=_wrap_new_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1348: flag=_wrap_delete_SixAxisForceTorqueSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1349: flag=_wrap_SixAxisForceTorqueSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1350: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1351: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1352: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1353: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1354: flag=_wrap_SixAxisForceTorqueSensor_setFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1355: flag=_wrap_SixAxisForceTorqueSensor_setSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1356: flag=_wrap_SixAxisForceTorqueSensor_getFirstLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1357: flag=_wrap_SixAxisForceTorqueSensor_getSecondLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1358: flag=_wrap_SixAxisForceTorqueSensor_setParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1359: flag=_wrap_SixAxisForceTorqueSensor_setParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1360: flag=_wrap_SixAxisForceTorqueSensor_setAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1361: flag=_wrap_SixAxisForceTorqueSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1362: flag=_wrap_SixAxisForceTorqueSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1363: flag=_wrap_SixAxisForceTorqueSensor_getParentJoint(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1364: flag=_wrap_SixAxisForceTorqueSensor_getParentJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1365: flag=_wrap_SixAxisForceTorqueSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1366: flag=_wrap_SixAxisForceTorqueSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1367: flag=_wrap_SixAxisForceTorqueSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1368: flag=_wrap_SixAxisForceTorqueSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1369: flag=_wrap_SixAxisForceTorqueSensor_getAppliedWrenchLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1370: flag=_wrap_SixAxisForceTorqueSensor_isLinkAttachedToSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1371: flag=_wrap_SixAxisForceTorqueSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1372: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1373: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1374: flag=_wrap_SixAxisForceTorqueSensor_getWrenchAppliedOnLinkInverseMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1375: flag=_wrap_SixAxisForceTorqueSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1376: flag=_wrap_SixAxisForceTorqueSensor_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1377: flag=_wrap_new_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1378: flag=_wrap_delete_AccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1379: flag=_wrap_AccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1380: flag=_wrap_AccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1381: flag=_wrap_AccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1382: flag=_wrap_AccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1383: flag=_wrap_AccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1384: flag=_wrap_AccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1385: flag=_wrap_AccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1386: flag=_wrap_AccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1387: flag=_wrap_AccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1388: flag=_wrap_AccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1389: flag=_wrap_AccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1390: flag=_wrap_AccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1391: flag=_wrap_AccelerometerSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1392: flag=_wrap_AccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1393: flag=_wrap_new_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1394: flag=_wrap_delete_GyroscopeSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1395: flag=_wrap_GyroscopeSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1396: flag=_wrap_GyroscopeSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1397: flag=_wrap_GyroscopeSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1398: flag=_wrap_GyroscopeSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1399: flag=_wrap_GyroscopeSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1400: flag=_wrap_GyroscopeSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1401: flag=_wrap_GyroscopeSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1402: flag=_wrap_GyroscopeSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1403: flag=_wrap_GyroscopeSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1404: flag=_wrap_GyroscopeSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1405: flag=_wrap_GyroscopeSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1406: flag=_wrap_GyroscopeSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1407: flag=_wrap_GyroscopeSensor_updateIndeces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1408: flag=_wrap_GyroscopeSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1409: flag=_wrap_new_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1410: flag=_wrap_delete_ThreeAxisAngularAccelerometerSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1411: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1412: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1413: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1414: flag=_wrap_ThreeAxisAngularAccelerometerSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1415: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1416: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1417: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1418: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1419: flag=_wrap_ThreeAxisAngularAccelerometerSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1420: flag=_wrap_ThreeAxisAngularAccelerometerSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1421: flag=_wrap_ThreeAxisAngularAccelerometerSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1422: flag=_wrap_ThreeAxisAngularAccelerometerSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1423: flag=_wrap_ThreeAxisAngularAccelerometerSensor_predictMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1424: flag=_wrap_new_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1425: flag=_wrap_delete_ThreeAxisForceTorqueContactSensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1426: flag=_wrap_ThreeAxisForceTorqueContactSensor_setName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1427: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1428: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1429: flag=_wrap_ThreeAxisForceTorqueContactSensor_setParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1430: flag=_wrap_ThreeAxisForceTorqueContactSensor_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1431: flag=_wrap_ThreeAxisForceTorqueContactSensor_getSensorType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1432: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1433: flag=_wrap_ThreeAxisForceTorqueContactSensor_getParentLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1434: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLinkSensorTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1435: flag=_wrap_ThreeAxisForceTorqueContactSensor_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1436: flag=_wrap_ThreeAxisForceTorqueContactSensor_clone(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1437: flag=_wrap_ThreeAxisForceTorqueContactSensor_updateIndices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1438: flag=_wrap_ThreeAxisForceTorqueContactSensor_setLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1439: flag=_wrap_ThreeAxisForceTorqueContactSensor_getLoadCellLocations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1440: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeThreeAxisForceTorqueFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1441: flag=_wrap_ThreeAxisForceTorqueContactSensor_computeCenterOfPressureFromLoadCellMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1442: flag=_wrap_predictSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1443: flag=_wrap_predictSensorsMeasurementsFromRawBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1444: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1445: flag=_wrap_URDFParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1446: flag=_wrap_URDFParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1447: flag=_wrap_URDFParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1448: flag=_wrap_new_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1449: flag=_wrap_delete_URDFParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1450: flag=_wrap_modelFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1451: flag=_wrap_modelFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1452: flag=_wrap_dofsListFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1453: flag=_wrap_dofsListFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1454: flag=_wrap_sensorsFromURDF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1455: flag=_wrap_sensorsFromURDFString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1456: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1457: flag=_wrap_ModelParserOptions_addSensorFramesAsAdditionalFrames_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1458: flag=_wrap_ModelParserOptions_originalFilename_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1459: flag=_wrap_ModelParserOptions_originalFilename_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1460: flag=_wrap_new_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1461: flag=_wrap_delete_ModelParserOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1462: flag=_wrap_new_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1463: flag=_wrap_delete_ModelLoader(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1464: flag=_wrap_ModelLoader_parsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1465: flag=_wrap_ModelLoader_setParsingOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1466: flag=_wrap_ModelLoader_loadModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1467: flag=_wrap_ModelLoader_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1468: flag=_wrap_ModelLoader_loadReducedModelFromFullModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1469: flag=_wrap_ModelLoader_loadReducedModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1470: flag=_wrap_ModelLoader_loadReducedModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1471: flag=_wrap_ModelLoader_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1472: flag=_wrap_ModelLoader_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1473: flag=_wrap_ModelLoader_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1474: flag=_wrap_ModelExporterOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1475: flag=_wrap_ModelExporterOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1476: flag=_wrap_ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1477: flag=_wrap_ModelExporterOptions_exportFirstBaseLinkAdditionalFrameAsFakeURDFBase_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1478: flag=_wrap_new_ModelExporterOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1479: flag=_wrap_delete_ModelExporterOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1480: flag=_wrap_new_ModelExporter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1481: flag=_wrap_delete_ModelExporter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1482: flag=_wrap_ModelExporter_exportingOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1483: flag=_wrap_ModelExporter_setExportingOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1484: flag=_wrap_ModelExporter_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1485: flag=_wrap_ModelExporter_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1486: flag=_wrap_ModelExporter_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1487: flag=_wrap_ModelExporter_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1488: flag=_wrap_ModelExporter_exportModelToString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1489: flag=_wrap_ModelExporter_exportModelToFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1490: flag=_wrap_new_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1491: flag=_wrap_UnknownWrenchContact_unknownType_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1492: flag=_wrap_UnknownWrenchContact_unknownType_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1493: flag=_wrap_UnknownWrenchContact_contactPoint_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1494: flag=_wrap_UnknownWrenchContact_contactPoint_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1495: flag=_wrap_UnknownWrenchContact_forceDirection_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1496: flag=_wrap_UnknownWrenchContact_forceDirection_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1497: flag=_wrap_UnknownWrenchContact_knownWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1498: flag=_wrap_UnknownWrenchContact_knownWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1499: flag=_wrap_UnknownWrenchContact_contactId_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1500: flag=_wrap_UnknownWrenchContact_contactId_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1501: flag=_wrap_delete_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1502: flag=_wrap_new_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1503: flag=_wrap_LinkUnknownWrenchContacts_clear(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1504: flag=_wrap_LinkUnknownWrenchContacts_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1505: flag=_wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1506: flag=_wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1507: flag=_wrap_LinkUnknownWrenchContacts_addNewContactForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1508: flag=_wrap_LinkUnknownWrenchContacts_addNewContactInFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1509: flag=_wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1510: flag=_wrap_LinkUnknownWrenchContacts_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1511: flag=_wrap_LinkUnknownWrenchContacts_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1512: flag=_wrap_delete_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1513: flag=_wrap_new_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1514: flag=_wrap_estimateExternalWrenchesBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1515: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1516: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1517: flag=_wrap_estimateExternalWrenchesBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1518: flag=_wrap_estimateExternalWrenchesBuffers_A_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1519: flag=_wrap_estimateExternalWrenchesBuffers_A_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1520: flag=_wrap_estimateExternalWrenchesBuffers_x_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1521: flag=_wrap_estimateExternalWrenchesBuffers_x_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1522: flag=_wrap_estimateExternalWrenchesBuffers_b_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1523: flag=_wrap_estimateExternalWrenchesBuffers_b_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1524: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1525: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1526: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1527: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1528: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1529: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1530: flag=_wrap_delete_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1531: flag=_wrap_estimateExternalWrenchesWithoutInternalFT(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1532: flag=_wrap_estimateExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1533: flag=_wrap_dynamicsEstimationForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1534: flag=_wrap_dynamicsEstimationForwardVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1535: flag=_wrap_computeLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1536: flag=_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1537: flag=_wrap_new_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1538: flag=_wrap_delete_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1539: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1540: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1541: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1542: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1543: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1544: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_submodels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1545: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1546: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1547: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1548: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1549: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1550: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1551: flag=_wrap_new_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1552: flag=_wrap_delete_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1553: flag=_wrap_SimpleLeggedOdometry_setModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1554: flag=_wrap_SimpleLeggedOdometry_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1555: flag=_wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1556: flag=_wrap_SimpleLeggedOdometry_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1557: flag=_wrap_SimpleLeggedOdometry_updateKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1558: flag=_wrap_SimpleLeggedOdometry_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1559: flag=_wrap_SimpleLeggedOdometry_changeFixedFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1560: flag=_wrap_SimpleLeggedOdometry_getCurrentFixedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1561: flag=_wrap_SimpleLeggedOdometry_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1562: flag=_wrap_SimpleLeggedOdometry_getWorldFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1563: flag=_wrap_isLinkBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1564: flag=_wrap_isJointBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1565: flag=_wrap_isDOFBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1566: flag=_wrap_new_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1567: flag=_wrap_BerdyOptions_berdyVariant_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1568: flag=_wrap_BerdyOptions_berdyVariant_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1569: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1570: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1571: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1572: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1573: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1574: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1575: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1576: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1577: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1578: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1579: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1580: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1581: flag=_wrap_BerdyOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1582: flag=_wrap_BerdyOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1583: flag=_wrap_BerdyOptions_checkConsistency(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1584: flag=_wrap_delete_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1585: flag=_wrap_BerdySensor_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1586: flag=_wrap_BerdySensor_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1587: flag=_wrap_BerdySensor_id_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1588: flag=_wrap_BerdySensor_id_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1589: flag=_wrap_BerdySensor_range_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1590: flag=_wrap_BerdySensor_range_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1591: flag=_wrap_BerdySensor_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1592: flag=_wrap_BerdySensor_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1593: flag=_wrap_new_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1594: flag=_wrap_delete_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1595: flag=_wrap_BerdyDynamicVariable_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1596: flag=_wrap_BerdyDynamicVariable_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1597: flag=_wrap_BerdyDynamicVariable_id_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1598: flag=_wrap_BerdyDynamicVariable_id_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1599: flag=_wrap_BerdyDynamicVariable_range_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1600: flag=_wrap_BerdyDynamicVariable_range_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1601: flag=_wrap_BerdyDynamicVariable_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1602: flag=_wrap_BerdyDynamicVariable_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1603: flag=_wrap_new_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1604: flag=_wrap_delete_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1605: flag=_wrap_new_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1606: flag=_wrap_BerdyHelper_dynamicTraversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1607: flag=_wrap_BerdyHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1608: flag=_wrap_BerdyHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1609: flag=_wrap_BerdyHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1610: flag=_wrap_BerdyHelper_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1611: flag=_wrap_BerdyHelper_getOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1612: flag=_wrap_BerdyHelper_getNrOfDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1613: flag=_wrap_BerdyHelper_getNrOfDynamicEquations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1614: flag=_wrap_BerdyHelper_getNrOfSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1615: flag=_wrap_BerdyHelper_resizeAndZeroBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1616: flag=_wrap_BerdyHelper_getBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1617: flag=_wrap_BerdyHelper_getSensorsOrdering(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1618: flag=_wrap_BerdyHelper_getRangeSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1619: flag=_wrap_BerdyHelper_getRangeDOFSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1620: flag=_wrap_BerdyHelper_getRangeJointSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1621: flag=_wrap_BerdyHelper_getRangeLinkSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1622: flag=_wrap_BerdyHelper_getRangeLinkVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1623: flag=_wrap_BerdyHelper_getRangeJointVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1624: flag=_wrap_BerdyHelper_getRangeDOFVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1625: flag=_wrap_BerdyHelper_getDynamicVariablesOrdering(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1626: flag=_wrap_BerdyHelper_serializeDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1627: flag=_wrap_BerdyHelper_serializeSensorVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1628: flag=_wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1629: flag=_wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1630: flag=_wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1631: flag=_wrap_BerdyHelper_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1632: flag=_wrap_BerdyHelper_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1633: flag=_wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1634: flag=_wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1635: flag=_wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1636: flag=_wrap_delete_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1637: flag=_wrap_new_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1638: flag=_wrap_delete_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1639: flag=_wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1640: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1641: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1642: flag=_wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1643: flag=_wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1644: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1645: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1646: flag=_wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1647: flag=_wrap_BerdySparseMAPSolver_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1648: flag=_wrap_BerdySparseMAPSolver_initialize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1649: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1650: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1651: flag=_wrap_BerdySparseMAPSolver_doEstimate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1652: flag=_wrap_BerdySparseMAPSolver_getLastEstimate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1653: flag=_wrap_AttitudeEstimatorState_m_orientation_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1654: flag=_wrap_AttitudeEstimatorState_m_orientation_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1655: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1656: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1657: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1658: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1659: flag=_wrap_new_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1660: flag=_wrap_delete_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1661: flag=_wrap_delete_IAttitudeEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1662: flag=_wrap_IAttitudeEstimator_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1663: flag=_wrap_IAttitudeEstimator_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1664: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1665: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1666: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1667: flag=_wrap_IAttitudeEstimator_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1668: flag=_wrap_IAttitudeEstimator_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1669: flag=_wrap_IAttitudeEstimator_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1670: flag=_wrap_IAttitudeEstimator_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1671: flag=_wrap_IAttitudeEstimator_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1672: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1673: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1674: flag=_wrap_AttitudeMahonyFilterParameters_kp_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1675: flag=_wrap_AttitudeMahonyFilterParameters_kp_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1676: flag=_wrap_AttitudeMahonyFilterParameters_ki_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1677: flag=_wrap_AttitudeMahonyFilterParameters_ki_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1678: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1679: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1680: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1681: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1682: flag=_wrap_new_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1683: flag=_wrap_delete_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1684: flag=_wrap_new_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1685: flag=_wrap_AttitudeMahonyFilter_useMagnetoMeterMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1686: flag=_wrap_AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1687: flag=_wrap_AttitudeMahonyFilter_setGainkp(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1688: flag=_wrap_AttitudeMahonyFilter_setGainki(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1689: flag=_wrap_AttitudeMahonyFilter_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1690: flag=_wrap_AttitudeMahonyFilter_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1691: flag=_wrap_AttitudeMahonyFilter_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1692: flag=_wrap_AttitudeMahonyFilter_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1693: flag=_wrap_AttitudeMahonyFilter_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1694: flag=_wrap_AttitudeMahonyFilter_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1695: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1696: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1697: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1698: flag=_wrap_AttitudeMahonyFilter_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1699: flag=_wrap_AttitudeMahonyFilter_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1700: flag=_wrap_AttitudeMahonyFilter_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1701: flag=_wrap_AttitudeMahonyFilter_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1702: flag=_wrap_AttitudeMahonyFilter_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1703: flag=_wrap_delete_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1704: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_f(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1705: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1706: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1707: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1708: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfPredict(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1709: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1710: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1711: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1712: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1713: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1714: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1715: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1716: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1717: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1718: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1719: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1720: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1721: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1722: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1723: flag=_wrap_delete_DiscreteExtendedKalmanFilterHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1724: flag=_wrap_output_dimensions_with_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1725: flag=_wrap_output_dimensions_without_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1726: flag=_wrap_input_dimensions_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1727: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1728: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1729: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1730: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1731: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1732: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1733: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1734: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1735: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1736: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1737: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1738: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1739: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1740: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1741: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1742: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1743: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1744: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1745: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1746: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1747: flag=_wrap_new_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1748: flag=_wrap_delete_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1749: flag=_wrap_new_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1750: flag=_wrap_AttitudeQuaternionEKF_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1751: flag=_wrap_AttitudeQuaternionEKF_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1752: flag=_wrap_AttitudeQuaternionEKF_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1753: flag=_wrap_AttitudeQuaternionEKF_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1754: flag=_wrap_AttitudeQuaternionEKF_setBiasCorrelationTimeFactor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1755: flag=_wrap_AttitudeQuaternionEKF_useMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1756: flag=_wrap_AttitudeQuaternionEKF_setMeasurementNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1757: flag=_wrap_AttitudeQuaternionEKF_setSystemNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1758: flag=_wrap_AttitudeQuaternionEKF_setInitialStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1759: flag=_wrap_AttitudeQuaternionEKF_initializeFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1760: flag=_wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1761: flag=_wrap_AttitudeQuaternionEKF_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1762: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1763: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1764: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1765: flag=_wrap_AttitudeQuaternionEKF_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1766: flag=_wrap_AttitudeQuaternionEKF_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1767: flag=_wrap_AttitudeQuaternionEKF_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1768: flag=_wrap_AttitudeQuaternionEKF_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1769: flag=_wrap_AttitudeQuaternionEKF_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1770: flag=_wrap_delete_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1771: flag=_wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1772: flag=_wrap_DynamicsRegressorParameter_category_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1773: flag=_wrap_DynamicsRegressorParameter_category_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1774: flag=_wrap_DynamicsRegressorParameter_elemIndex_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1775: flag=_wrap_DynamicsRegressorParameter_elemIndex_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1776: flag=_wrap_DynamicsRegressorParameter_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1777: flag=_wrap_DynamicsRegressorParameter_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1778: flag=_wrap_DynamicsRegressorParameter_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1779: flag=_wrap_DynamicsRegressorParameter_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1780: flag=_wrap_DynamicsRegressorParameter_ne(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1781: flag=_wrap_new_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1782: flag=_wrap_delete_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1783: flag=_wrap_DynamicsRegressorParametersList_parameters_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1784: flag=_wrap_DynamicsRegressorParametersList_parameters_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1785: flag=_wrap_DynamicsRegressorParametersList_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1786: flag=_wrap_DynamicsRegressorParametersList_addParam(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1787: flag=_wrap_DynamicsRegressorParametersList_addList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1788: flag=_wrap_DynamicsRegressorParametersList_findParam(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1789: flag=_wrap_DynamicsRegressorParametersList_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1790: flag=_wrap_new_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1791: flag=_wrap_delete_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1792: flag=_wrap_new_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1793: flag=_wrap_delete_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1794: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1795: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1796: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1797: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1798: flag=_wrap_DynamicsRegressorGenerator_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1799: flag=_wrap_DynamicsRegressorGenerator_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1800: flag=_wrap_DynamicsRegressorGenerator_getNrOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1801: flag=_wrap_DynamicsRegressorGenerator_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1802: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1803: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1804: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutput(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1805: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1806: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1807: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1808: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1809: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1810: flag=_wrap_DynamicsRegressorGenerator_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1811: flag=_wrap_DynamicsRegressorGenerator_getNrOfFakeLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1812: flag=_wrap_DynamicsRegressorGenerator_getBaseLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1813: flag=_wrap_DynamicsRegressorGenerator_getSensorsModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1814: flag=_wrap_DynamicsRegressorGenerator_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1815: flag=_wrap_DynamicsRegressorGenerator_getSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1816: flag=_wrap_DynamicsRegressorGenerator_setTorqueSensorMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1817: flag=_wrap_DynamicsRegressorGenerator_computeRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1818: flag=_wrap_DynamicsRegressorGenerator_getModelParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1819: flag=_wrap_DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1820: flag=_wrap_DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1821: flag=_wrap_DynamicsRegressorGenerator_generate_random_regressors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1822: flag=_wrap_new_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1823: flag=_wrap_delete_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1824: flag=_wrap_KinDynComputations_loadRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1825: flag=_wrap_KinDynComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1826: flag=_wrap_KinDynComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1827: flag=_wrap_KinDynComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1828: flag=_wrap_KinDynComputations_setFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1829: flag=_wrap_KinDynComputations_getFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1830: flag=_wrap_KinDynComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1831: flag=_wrap_KinDynComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1832: flag=_wrap_KinDynComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1833: flag=_wrap_KinDynComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1834: flag=_wrap_KinDynComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1835: flag=_wrap_KinDynComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1836: flag=_wrap_KinDynComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1837: flag=_wrap_KinDynComputations_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1838: flag=_wrap_KinDynComputations_getRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1839: flag=_wrap_KinDynComputations_getRelativeJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1840: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1841: flag=_wrap_KinDynComputations_setJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1842: flag=_wrap_KinDynComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1843: flag=_wrap_KinDynComputations_getRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1844: flag=_wrap_KinDynComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1845: flag=_wrap_KinDynComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1846: flag=_wrap_KinDynComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1847: flag=_wrap_KinDynComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1848: flag=_wrap_KinDynComputations_getModelVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1849: flag=_wrap_KinDynComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1850: flag=_wrap_KinDynComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1851: flag=_wrap_KinDynComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1852: flag=_wrap_KinDynComputations_getRelativeTransformExplicit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1853: flag=_wrap_KinDynComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1854: flag=_wrap_KinDynComputations_getFrameVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1855: flag=_wrap_KinDynComputations_getFrameAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1856: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1857: flag=_wrap_KinDynComputations_getRelativeJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1858: flag=_wrap_KinDynComputations_getRelativeJacobianExplicit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1859: flag=_wrap_KinDynComputations_getFrameBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1860: flag=_wrap_KinDynComputations_getCenterOfMassPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1861: flag=_wrap_KinDynComputations_getCenterOfMassVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1862: flag=_wrap_KinDynComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1863: flag=_wrap_KinDynComputations_getCenterOfMassBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1864: flag=_wrap_KinDynComputations_getAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1865: flag=_wrap_KinDynComputations_getAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1866: flag=_wrap_KinDynComputations_getCentroidalAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1867: flag=_wrap_KinDynComputations_getCentroidalAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1868: flag=_wrap_KinDynComputations_getLinearAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1869: flag=_wrap_KinDynComputations_getLinearAngularMomentumJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1870: flag=_wrap_KinDynComputations_getCentroidalTotalMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1871: flag=_wrap_KinDynComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1872: flag=_wrap_KinDynComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1873: flag=_wrap_KinDynComputations_generalizedBiasForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1874: flag=_wrap_KinDynComputations_generalizedGravityForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1875: flag=_wrap_KinDynComputations_generalizedExternalForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1876: flag=_wrap_KinDynComputations_inverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1877: flag=_wrap_delete_ICamera(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1878: flag=_wrap_ICamera_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1879: flag=_wrap_ICamera_setTarget(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1880: flag=_wrap_ICamera_setUpVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1881: flag=_wrap_ColorViz_r_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1882: flag=_wrap_ColorViz_r_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1883: flag=_wrap_ColorViz_g_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1884: flag=_wrap_ColorViz_g_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1885: flag=_wrap_ColorViz_b_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1886: flag=_wrap_ColorViz_b_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1887: flag=_wrap_ColorViz_a_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1888: flag=_wrap_ColorViz_a_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1889: flag=_wrap_new_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1890: flag=_wrap_delete_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1891: flag=_wrap_delete_ILight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1892: flag=_wrap_ILight_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1893: flag=_wrap_ILight_setType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1894: flag=_wrap_ILight_getType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1895: flag=_wrap_ILight_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1896: flag=_wrap_ILight_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1897: flag=_wrap_ILight_setDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1898: flag=_wrap_ILight_getDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1899: flag=_wrap_ILight_setAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1900: flag=_wrap_ILight_getAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1901: flag=_wrap_ILight_setSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1902: flag=_wrap_ILight_getSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1903: flag=_wrap_ILight_setDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1904: flag=_wrap_ILight_getDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1905: flag=_wrap_delete_IEnvironment(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1906: flag=_wrap_IEnvironment_getElements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1907: flag=_wrap_IEnvironment_setElementVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1908: flag=_wrap_IEnvironment_setBackgroundColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1909: flag=_wrap_IEnvironment_setAmbientLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1910: flag=_wrap_IEnvironment_getLights(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1911: flag=_wrap_IEnvironment_addLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1912: flag=_wrap_IEnvironment_lightViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1913: flag=_wrap_IEnvironment_removeLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1914: flag=_wrap_delete_IJetsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1915: flag=_wrap_IJetsVisualization_setJetsFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1916: flag=_wrap_IJetsVisualization_getNrOfJets(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1917: flag=_wrap_IJetsVisualization_getJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1918: flag=_wrap_IJetsVisualization_setJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1919: flag=_wrap_IJetsVisualization_setJetColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1920: flag=_wrap_IJetsVisualization_setJetsDimensions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1921: flag=_wrap_IJetsVisualization_setJetsIntensity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1922: flag=_wrap_delete_IVectorsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1923: flag=_wrap_IVectorsVisualization_addVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1924: flag=_wrap_IVectorsVisualization_getNrOfVectors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1925: flag=_wrap_IVectorsVisualization_getVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1926: flag=_wrap_IVectorsVisualization_updateVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1927: flag=_wrap_IVectorsVisualization_setVectorColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1928: flag=_wrap_IVectorsVisualization_setVectorsAspect(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1929: flag=_wrap_delete_IModelVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1930: flag=_wrap_IModelVisualization_setPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1931: flag=_wrap_IModelVisualization_setLinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1932: flag=_wrap_IModelVisualization_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1933: flag=_wrap_IModelVisualization_getInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1934: flag=_wrap_IModelVisualization_setModelVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1935: flag=_wrap_IModelVisualization_setModelColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1936: flag=_wrap_IModelVisualization_resetModelColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1937: flag=_wrap_IModelVisualization_setLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1938: flag=_wrap_IModelVisualization_resetLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1939: flag=_wrap_IModelVisualization_getLinkNames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1940: flag=_wrap_IModelVisualization_setLinkVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1941: flag=_wrap_IModelVisualization_getFeatures(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1942: flag=_wrap_IModelVisualization_setFeatureVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1943: flag=_wrap_IModelVisualization_jets(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1944: flag=_wrap_IModelVisualization_getWorldModelTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1945: flag=_wrap_IModelVisualization_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1946: flag=_wrap_VisualizerOptions_verbose_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1947: flag=_wrap_VisualizerOptions_verbose_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1948: flag=_wrap_VisualizerOptions_winWidth_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1949: flag=_wrap_VisualizerOptions_winWidth_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1950: flag=_wrap_VisualizerOptions_winHeight_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1951: flag=_wrap_VisualizerOptions_winHeight_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1952: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1953: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1954: flag=_wrap_new_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1955: flag=_wrap_delete_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1956: flag=_wrap_new_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1957: flag=_wrap_delete_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1958: flag=_wrap_Visualizer_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1959: flag=_wrap_Visualizer_getNrOfVisualizedModels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1960: flag=_wrap_Visualizer_getModelInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1961: flag=_wrap_Visualizer_getModelInstanceIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1962: flag=_wrap_Visualizer_addModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1963: flag=_wrap_Visualizer_modelViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1964: flag=_wrap_Visualizer_camera(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1965: flag=_wrap_Visualizer_enviroment(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1966: flag=_wrap_Visualizer_vectors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1967: flag=_wrap_Visualizer_run(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1968: flag=_wrap_Visualizer_draw(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1969: flag=_wrap_Visualizer_drawToFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1970: flag=_wrap_Visualizer_close(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1971: flag=_wrap_new_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1972: flag=_wrap_delete_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1973: flag=_wrap_DynamicsComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1974: flag=_wrap_DynamicsComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1975: flag=_wrap_DynamicsComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1976: flag=_wrap_DynamicsComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1977: flag=_wrap_DynamicsComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1978: flag=_wrap_DynamicsComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1979: flag=_wrap_DynamicsComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1980: flag=_wrap_DynamicsComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1981: flag=_wrap_DynamicsComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1982: flag=_wrap_DynamicsComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1983: flag=_wrap_DynamicsComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1984: flag=_wrap_DynamicsComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1985: flag=_wrap_DynamicsComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1986: flag=_wrap_DynamicsComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1987: flag=_wrap_DynamicsComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1988: flag=_wrap_DynamicsComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1989: flag=_wrap_DynamicsComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1990: flag=_wrap_DynamicsComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1991: flag=_wrap_DynamicsComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1992: flag=_wrap_DynamicsComputations_getFrameTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1993: flag=_wrap_DynamicsComputations_getFrameTwistInWorldOrient(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1994: flag=_wrap_DynamicsComputations_getFrameProperSpatialAcceleration(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1995: flag=_wrap_DynamicsComputations_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1996: flag=_wrap_DynamicsComputations_getLinkInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1997: flag=_wrap_DynamicsComputations_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1998: flag=_wrap_DynamicsComputations_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1999: flag=_wrap_DynamicsComputations_getJointLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2000: flag=_wrap_DynamicsComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2001: flag=_wrap_DynamicsComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2002: flag=_wrap_DynamicsComputations_getFrameJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2003: flag=_wrap_DynamicsComputations_getDynamicsRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2004: flag=_wrap_DynamicsComputations_getModelDynamicsParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2005: flag=_wrap_DynamicsComputations_getCenterOfMass(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2006: flag=_wrap_DynamicsComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
   default: flag=1, SWIG_Error(SWIG_RuntimeError, "No function id %d.", fcn_id);
   }
   if (flag) {

From 142bf0545b29f47c11325458657e779532bf9810 Mon Sep 17 00:00:00 2001
From: Prashanth <prashanthramadoss@gmail.com>
Date: Fri, 27 Sep 2019 13:13:40 +0200
Subject: [PATCH 151/194] [doc] Add simple tutorial for generating matlab
 bindings

---
 doc/dev/faqs.md                            |  2 +-
 doc/generating-idyntree-matlab-bindings.md | 58 ++++++++++++++++++++++
 2 files changed, 59 insertions(+), 1 deletion(-)
 create mode 100644 doc/generating-idyntree-matlab-bindings.md

diff --git a/doc/dev/faqs.md b/doc/dev/faqs.md
index 29df1e3a25b..e85231f5359 100644
--- a/doc/dev/faqs.md
+++ b/doc/dev/faqs.md
@@ -35,7 +35,7 @@ with your GitHub account, and you will automatically receive notifications if yo
 * Check if Travis and AppVeyor compiler and run the test without any failure.  
 
 ## How to add wrap a new class or function with SWIG 
-* Include the new header in [bindings/iDynTree.i](bindings/iDynTree.i) . 
+* Include the new header in [bindings/iDynTree.i](bindings/iDynTree.i) .  For a little more detailed overview, check [doc/generating-idyntree-matlab-bindings.md](../../doc/generating-idyntree-matlab-bindings.md) . 
   * Notice that the headers should be included two times, take inspiration from the headers already present to get the idea.
   * Remember to include the headers following the inheritance order of the classes. 
 * If you add a instantiated template, remember to add it to swig with the `%template` SWIG command. 
diff --git a/doc/generating-idyntree-matlab-bindings.md b/doc/generating-idyntree-matlab-bindings.md
new file mode 100644
index 00000000000..8d18027961d
--- /dev/null
+++ b/doc/generating-idyntree-matlab-bindings.md
@@ -0,0 +1,58 @@
+
+## Generating Matlab bindings for iDynTree Classes
+
+As per the documentation of [iDynTree](https://github.com/robotology/idyntree) for [modifications of Matlab/Octave bindings](https://github.com/robotology/idyntree#matlaboctave-bindings-modifications), the prerequisite is an experimental version of [Swig](http://www.swig.org/svn.html) with Matlab support.
+This can be found in the repository [https://github.com/robotology-dependencies/swig/](https://github.com/robotology-dependencies/swig/) (branch `matlab`).
+
+## Installing experimental version of Swig
+- Clone the repository from [https://github.com/robotology-dependencies/swig/](https://github.com/robotology-dependencies/swig/)
+- Install the required dependencies for SWIG listed in the **Required Tools** section of the [Swig page](http://www.swig.org/svn.html).
+- Follow the installation instructions in [http://www.swig.org/Doc3.0/SWIGDocumentation.html#Preface_unix_installation](http://www.swig.org/Doc3.0/SWIGDocumentation.html#Preface_unix_installation) to install Swig locally. By default SWIG installs itself in `/usr/local`, in Unix environment. However, it is preferable to install Swig in a different location so as to avoid conflicts with other installed versions of SWIG.
+- Proper installation of SWIG will generate a `swig` executable in `<swig_install_dir>/bin`.
+
+This executable will be used to generate the iDynTree Matlab/Octave bindings for custom classes.
+
+## Including the custom classes for bindings generations
+In this section, as a toy example, let us consider a custom class `DiscreteKalmanFilterHelper` for which the bindings need to be generated. Let's assume the class `DiscreteKalmanFilterHelper` exists in the header file `iDynTree/Estimation/KalmanFilter.h`.
+
+- Take a glance at `bindings/iDynTree.i`. It follows a particular structure, adding the necessary header files twice for the desired classes, one using the prefix `#` and another using the prefix `%`.
+    - It is also important to notice the order in which these headers are added, which ensures that the dependent header file is compiled after the dependee header file.
+    - There are also sections for adding library specific data-structure; sensors-related, estimation-related, etc.
+    - Templatized classes are explicitly specialized, so that the bindings are generated for the template specializations.
+
+- Coming back to our toy example, we will add the header file in the section related to the `Estimation related classes`, in two different places
+```
+#include "iDynTree/Estimation/KalmanFilter.h"
+```
+and
+```
+%include "iDynTree/Estimation/KalmanFilter.h"
+```
+
+Now, all is left is to compile and generate the bindings.
+
+## Generating the bindings
+- In the build folder of `iDynTree`, run `ccmake` to open the CMake Gui.
+    - Set the CMake option `SWIG_EXECUTABLE` to `<swig_install_dir>/bin/swig`.
+    - Set the CMake option `IDYNTREE_GENERATE_MATLAB` to `ON`.
+    - Set the CMake option `IDYNTREE_USES_MATLAB` or `IDYNTREE_USES_OCTAVE` to `ON`.
+- Having configured the CMake, run `make` and `make install` to install the bindings in the `mex` directory of the iDynTree install location.
+
+__NOTE__: It is possible that you might face some compiler warnings or errors which needs to be handled properly for successful generation.
+
+- Now, it is possible to use the `iDynTree.DiscreteKalmanFilterHelper` object in Matlab environment, if Matlab is configured to use the Matlab bindings as described in [iDynTree/Bindings](https://github.com/robotology/idyntree#bindings) section.
+
+- Additionally, you will notice in the source folder that the bindings for other classes (not only related to `DiscreteKalmanFilterHelper`) have been modified in `iDynTree/bindings/matlab/autogenerated/+iDynTree/`. This is because the generation of the bindings, modifies a certain function numbering in all of these files, and the class related methods in Matlab are called depending on this specific numbering.
+
+## Adding the generated bindings to the iDynTree Library
+Usually, the generation of bindings is only allowed by the developers. So in order to create a pull request to add the custom generated bindings to iDynTree, we will follow some contribution instructions.
+- Along with the modified `bindings/iDynTree.i` file, commit all the autogenerated binding files, not only the changes related specifically to your class. The latter might break the complete usage of the bindings due to the dependence on certain function numbering followed by SWIG.
+- Keep only the bindings related changes in a single commit with the commit message `[bindings] update matlab bindings`, if you're generating matlab bindings. Similar message applies for python or octave bindings.
+- Open a Pull request and wait for review!
+
+## Ending note - IMPORTANT
+In this tutorial, we addressed a basic feature of generating bindings for a simple class.
+It must be noted that there are certain additional rules that need to be followed to generate bindings for templatized classes.
+Additionally, there are certain features that the SWIG does not support, for example, bindings cannot be generated for nested classes. In general, it is useful to check out the [SWIG documentation for C++](http://www.swig.org/Doc1.3/SWIGPlus.html).
+
+It might also be helpful to take a look into the files `iDynTree/bindings/ignore.i`, `iDynTree/bindings/sensors.i`, etc. to handle specific use cases. In particular, these are used to ignore certain methods for which the bindings need not be generated or to extend the functionality of certain classes using methods that needs to be explicitly generated as bindings only.

From c3267b7bdeda1366317c44bbab4e628409017663 Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Thu, 3 Oct 2019 10:57:27 +0200
Subject: [PATCH 152/194] fix missing DOF_ACCELERATION to dynamic variable
 ordering

---
 src/estimation/src/BerdyHelper.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/estimation/src/BerdyHelper.cpp b/src/estimation/src/BerdyHelper.cpp
index 4dc3fbdea46..e0d9013cc6e 100644
--- a/src/estimation/src/BerdyHelper.cpp
+++ b/src/estimation/src/BerdyHelper.cpp
@@ -1807,6 +1807,8 @@ bool BerdyHelper::getBerdyMatrices(SparseMatrix<iDynTree::ColumnMajor>& D, Vecto
                 dofAcceleration.type = DOF_ACCELERATION;
                 dofAcceleration.id = m_model.getJointName(jntIdx);
                 dofAcceleration.range = getRangeDOFVariable(dofAcceleration.type, joint->getDOFsOffset());
+
+                m_dynamicVariablesOrdering.push_back(dofAcceleration);
             }
         }
 

From 825a69957f6d3f95f1361fe6c1f3a2eacc4b81ca Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 3 Oct 2019 14:51:25 +0200
Subject: [PATCH 153/194] Add ModelCalibrationHelper to SWIG bindings

---
 bindings/iDynTree.i | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/bindings/iDynTree.i b/bindings/iDynTree.i
index 92ea79ec347..67c2ebda518 100644
--- a/bindings/iDynTree.i
+++ b/bindings/iDynTree.i
@@ -109,6 +109,8 @@
 #include "iDynTree/ModelIO/URDFGenericSensorsImport.h"
 #include "iDynTree/ModelIO/ModelLoader.h"
 #include "iDynTree/ModelIO/ModelExporter.h"
+#include "iDynTree/ModelIO/ModelCalibrationHelper.h"
+
 
 // Estimation related classes
 #include "iDynTree/Estimation/ExternalWrenchesEstimation.h"
@@ -317,6 +319,7 @@ TEMPLATE_WRAP_MOTION_FORCE(ForceVector3, WRAP_FORCE, SET_NAME_FOR_WRAPPER,,)
 %include "iDynTree/ModelIO/URDFGenericSensorsImport.h"
 %include "iDynTree/ModelIO/ModelLoader.h"
 %include "iDynTree/ModelIO/ModelExporter.h"
+%include "iDynTree/ModelIO/ModelCalibrationHelper.h"
 
 // Estimation related classes
 %include "iDynTree/Estimation/ExternalWrenchesEstimation.h"

From 7b81fc278665ecbf9e7f2adba66d344ae2e93e31 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 3 Oct 2019 14:51:59 +0200
Subject: [PATCH 154/194] [bindings] update matlab bindings

---
 .../+iDynTree/AttitudeEstimatorState.m        |   16 +-
 .../+iDynTree/AttitudeMahonyFilter.m          |   40 +-
 .../AttitudeMahonyFilterParameters.m          |   24 +-
 .../+iDynTree/AttitudeQuaternionEKF.m         |   44 +-
 .../AttitudeQuaternionEKFParameters.m         |   44 +-
 .../+iDynTree/BerdyDynamicVariable.m          |   20 +-
 .../autogenerated/+iDynTree/BerdyHelper.m     |   64 +-
 .../autogenerated/+iDynTree/BerdyOptions.m    |   38 +-
 .../autogenerated/+iDynTree/BerdySensor.m     |   20 +-
 .../+iDynTree/BerdySparseMAPSolver.m          |   32 +-
 .../matlab/autogenerated/+iDynTree/ColorViz.m |   20 +-
 .../DiscreteExtendedKalmanFilterHelper.m      |   40 +-
 .../+iDynTree/DynamicsComputations.m          |   72 +-
 .../+iDynTree/DynamicsRegressorGenerator.m    |   60 +-
 .../+iDynTree/DynamicsRegressorParameter.m    |   22 +-
 .../DynamicsRegressorParametersList.m         |   18 +-
 .../ExtWrenchesAndJointTorquesEstimator.m     |   28 +-
 .../+iDynTree/IAttitudeEstimator.m            |   22 +-
 .../matlab/autogenerated/+iDynTree/ICamera.m  |    8 +-
 .../autogenerated/+iDynTree/IEnvironment.m    |   18 +-
 .../+iDynTree/IJetsVisualization.m            |   16 +-
 .../matlab/autogenerated/+iDynTree/ILight.m   |   28 +-
 .../+iDynTree/IModelVisualization.m           |   34 +-
 .../+iDynTree/IVectorsVisualization.m         |   14 +-
 .../+iDynTree/KinDynComputations.m            |  110 +-
 .../+iDynTree/LinkUnknownWrenchContacts.m     |   22 +-
 .../+iDynTree/ModelCalibrationHelper.m        |   47 +
 .../+iDynTree/SimpleLeggedOdometry.m          |   24 +-
 .../+iDynTree/UnknownWrenchContact.m          |   24 +-
 .../autogenerated/+iDynTree/Visualizer.m      |   30 +-
 .../+iDynTree/VisualizerOptions.m             |   20 +-
 .../computeLinkNetWrenchesWithoutGravity.m    |    2 +-
 ...ynamicsEstimationForwardVelAccKinematics.m |    2 +-
 .../dynamicsEstimationForwardVelKinematics.m  |    2 +-
 .../+iDynTree/estimateExternalWrenches.m      |    2 +-
 .../estimateExternalWrenchesBuffers.m         |   36 +-
 ...stimateExternalWrenchesWithoutInternalFT.m |    2 +-
 ...ametersFromLinkBoundingBoxesAndTotalMass.m |    2 +-
 ...ntactWrenchesFromLinkNetExternalWrenches.m |    2 +-
 .../+iDynTree/input_dimensions.m              |    2 +-
 .../+iDynTree/isDOFBerdyDynamicVariable.m     |    2 +-
 .../+iDynTree/isJointBerdyDynamicVariable.m   |    2 +-
 .../+iDynTree/isLinkBerdyDynamicVariable.m    |    2 +-
 .../output_dimensions_with_magnetometer.m     |    2 +-
 .../output_dimensions_without_magnetometer.m  |    2 +-
 .../autogenerated/iDynTreeMATLAB_wrap.cxx     | 3194 +++++++++++------
 46 files changed, 2622 insertions(+), 1653 deletions(-)
 create mode 100644 bindings/matlab/autogenerated/+iDynTree/ModelCalibrationHelper.m

diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
index 9a09409acd4..d9d280f36f4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeEstimatorState.m
@@ -7,30 +7,30 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1653, self);
+        varargout{1} = iDynTreeMEX(1662, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1654, self, varargin{1});
+        iDynTreeMEX(1663, self, varargin{1});
       end
     end
     function varargout = m_angular_velocity(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1655, self);
+        varargout{1} = iDynTreeMEX(1664, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1656, self, varargin{1});
+        iDynTreeMEX(1665, self, varargin{1});
       end
     end
     function varargout = m_gyroscope_bias(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1657, self);
+        varargout{1} = iDynTreeMEX(1666, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1658, self, varargin{1});
+        iDynTreeMEX(1667, self, varargin{1});
       end
     end
     function self = AttitudeEstimatorState(varargin)
@@ -39,14 +39,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1659, varargin{:});
+        tmp = iDynTreeMEX(1668, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1660, self);
+        iDynTreeMEX(1669, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
index 2e3d1f16256..dacfd57cf1d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilter.m
@@ -7,68 +7,68 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1684, varargin{:});
+        tmp = iDynTreeMEX(1693, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = useMagnetoMeterMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1685, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1694, self, varargin{:});
     end
     function varargout = setConfidenceForMagnetometerMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1686, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1695, self, varargin{:});
     end
     function varargout = setGainkp(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1687, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1696, self, varargin{:});
     end
     function varargout = setGainki(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1688, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1697, self, varargin{:});
     end
     function varargout = setTimeStepInSeconds(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1689, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1698, self, varargin{:});
     end
     function varargout = setGravityDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1690, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1699, self, varargin{:});
     end
     function varargout = setParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1691, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1700, self, varargin{:});
     end
     function varargout = getParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1692, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1701, self, varargin{:});
     end
     function varargout = updateFilterWithMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1693, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1702, self, varargin{:});
     end
     function varargout = propagateStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1694, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1703, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1695, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1704, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsQuaternion(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1696, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1705, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRPY(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1697, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1706, self, varargin{:});
     end
     function varargout = getInternalStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1698, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1707, self, varargin{:});
     end
     function varargout = getInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1699, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1708, self, varargin{:});
     end
     function varargout = getDefaultInternalInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1700, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1709, self, varargin{:});
     end
     function varargout = setInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1701, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1710, self, varargin{:});
     end
     function varargout = setInternalStateInitialOrientation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1702, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1711, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1703, self);
+        iDynTreeMEX(1712, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
index ad3303e74c0..a74e05bab83 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeMahonyFilterParameters.m
@@ -7,50 +7,50 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1672, self);
+        varargout{1} = iDynTreeMEX(1681, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1673, self, varargin{1});
+        iDynTreeMEX(1682, self, varargin{1});
       end
     end
     function varargout = kp(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1674, self);
+        varargout{1} = iDynTreeMEX(1683, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1675, self, varargin{1});
+        iDynTreeMEX(1684, self, varargin{1});
       end
     end
     function varargout = ki(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1676, self);
+        varargout{1} = iDynTreeMEX(1685, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1677, self, varargin{1});
+        iDynTreeMEX(1686, self, varargin{1});
       end
     end
     function varargout = use_magnetometer_measurements(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1678, self);
+        varargout{1} = iDynTreeMEX(1687, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1679, self, varargin{1});
+        iDynTreeMEX(1688, self, varargin{1});
       end
     end
     function varargout = confidence_magnetometer_measurements(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1680, self);
+        varargout{1} = iDynTreeMEX(1689, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1681, self, varargin{1});
+        iDynTreeMEX(1690, self, varargin{1});
       end
     end
     function self = AttitudeMahonyFilterParameters(varargin)
@@ -59,14 +59,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1682, varargin{:});
+        tmp = iDynTreeMEX(1691, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1683, self);
+        iDynTreeMEX(1692, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
index 0072f02c5c5..edbc6798ac0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKF.m
@@ -11,74 +11,74 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1749, varargin{:});
+        tmp = iDynTreeMEX(1758, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = getParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1750, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1759, self, varargin{:});
     end
     function varargout = setParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1751, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1760, self, varargin{:});
     end
     function varargout = setGravityDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1752, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1761, self, varargin{:});
     end
     function varargout = setTimeStepInSeconds(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1753, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1762, self, varargin{:});
     end
     function varargout = setBiasCorrelationTimeFactor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1754, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1763, self, varargin{:});
     end
     function varargout = useMagnetometerMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1755, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1764, self, varargin{:});
     end
     function varargout = setMeasurementNoiseVariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1756, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1765, self, varargin{:});
     end
     function varargout = setSystemNoiseVariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1757, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1766, self, varargin{:});
     end
     function varargout = setInitialStateCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1758, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1767, self, varargin{:});
     end
     function varargout = initializeFilter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1759, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1768, self, varargin{:});
     end
     function varargout = updateFilterWithMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1760, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1769, self, varargin{:});
     end
     function varargout = propagateStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1761, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1770, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1762, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1771, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsQuaternion(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1763, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1772, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRPY(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1764, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1773, self, varargin{:});
     end
     function varargout = getInternalStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1765, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1774, self, varargin{:});
     end
     function varargout = getInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1766, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1775, self, varargin{:});
     end
     function varargout = getDefaultInternalInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1767, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1776, self, varargin{:});
     end
     function varargout = setInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1768, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1777, self, varargin{:});
     end
     function varargout = setInternalStateInitialOrientation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1769, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1778, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1770, self);
+        iDynTreeMEX(1779, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
index fe645eab69a..d693728f579 100644
--- a/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
+++ b/bindings/matlab/autogenerated/+iDynTree/AttitudeQuaternionEKFParameters.m
@@ -7,100 +7,100 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1727, self);
+        varargout{1} = iDynTreeMEX(1736, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1728, self, varargin{1});
+        iDynTreeMEX(1737, self, varargin{1});
       end
     end
     function varargout = bias_correlation_time_factor(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1729, self);
+        varargout{1} = iDynTreeMEX(1738, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1730, self, varargin{1});
+        iDynTreeMEX(1739, self, varargin{1});
       end
     end
     function varargout = accelerometer_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1731, self);
+        varargout{1} = iDynTreeMEX(1740, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1732, self, varargin{1});
+        iDynTreeMEX(1741, self, varargin{1});
       end
     end
     function varargout = magnetometer_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1733, self);
+        varargout{1} = iDynTreeMEX(1742, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1734, self, varargin{1});
+        iDynTreeMEX(1743, self, varargin{1});
       end
     end
     function varargout = gyroscope_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1735, self);
+        varargout{1} = iDynTreeMEX(1744, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1736, self, varargin{1});
+        iDynTreeMEX(1745, self, varargin{1});
       end
     end
     function varargout = gyro_bias_noise_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1737, self);
+        varargout{1} = iDynTreeMEX(1746, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1738, self, varargin{1});
+        iDynTreeMEX(1747, self, varargin{1});
       end
     end
     function varargout = initial_orientation_error_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1739, self);
+        varargout{1} = iDynTreeMEX(1748, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1740, self, varargin{1});
+        iDynTreeMEX(1749, self, varargin{1});
       end
     end
     function varargout = initial_ang_vel_error_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1741, self);
+        varargout{1} = iDynTreeMEX(1750, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1742, self, varargin{1});
+        iDynTreeMEX(1751, self, varargin{1});
       end
     end
     function varargout = initial_gyro_bias_error_variance(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1743, self);
+        varargout{1} = iDynTreeMEX(1752, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1744, self, varargin{1});
+        iDynTreeMEX(1753, self, varargin{1});
       end
     end
     function varargout = use_magnetometer_measurements(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1745, self);
+        varargout{1} = iDynTreeMEX(1754, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1746, self, varargin{1});
+        iDynTreeMEX(1755, self, varargin{1});
       end
     end
     function self = AttitudeQuaternionEKFParameters(varargin)
@@ -109,14 +109,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1747, varargin{:});
+        tmp = iDynTreeMEX(1756, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1748, self);
+        iDynTreeMEX(1757, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
index eeae97d5cfa..24d95c314a5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyDynamicVariable.m
@@ -7,37 +7,37 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1595, self);
+        varargout{1} = iDynTreeMEX(1604, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1596, self, varargin{1});
+        iDynTreeMEX(1605, self, varargin{1});
       end
     end
     function varargout = id(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1597, self);
+        varargout{1} = iDynTreeMEX(1606, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1598, self, varargin{1});
+        iDynTreeMEX(1607, self, varargin{1});
       end
     end
     function varargout = range(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1599, self);
+        varargout{1} = iDynTreeMEX(1608, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1600, self, varargin{1});
+        iDynTreeMEX(1609, self, varargin{1});
       end
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1601, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1610, self, varargin{:});
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1602, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1611, self, varargin{:});
     end
     function self = BerdyDynamicVariable(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -45,14 +45,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1603, varargin{:});
+        tmp = iDynTreeMEX(1612, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1604, self);
+        iDynTreeMEX(1613, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m b/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
index bb092a248fc..d0abe70d91d 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyHelper.m
@@ -9,104 +9,104 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1605, varargin{:});
+        tmp = iDynTreeMEX(1614, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = dynamicTraversal(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1606, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1615, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1607, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1616, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1608, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1617, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1609, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1618, self, varargin{:});
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1610, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1619, self, varargin{:});
     end
     function varargout = getOptions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1611, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1620, self, varargin{:});
     end
     function varargout = getNrOfDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1612, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1621, self, varargin{:});
     end
     function varargout = getNrOfDynamicEquations(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1613, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1622, self, varargin{:});
     end
     function varargout = getNrOfSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1614, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1623, self, varargin{:});
     end
     function varargout = resizeAndZeroBerdyMatrices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1615, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1624, self, varargin{:});
     end
     function varargout = getBerdyMatrices(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1616, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1625, self, varargin{:});
     end
     function varargout = getSensorsOrdering(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1617, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1626, self, varargin{:});
     end
     function varargout = getRangeSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1618, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1627, self, varargin{:});
     end
     function varargout = getRangeDOFSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1619, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1628, self, varargin{:});
     end
     function varargout = getRangeJointSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1620, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1629, self, varargin{:});
     end
     function varargout = getRangeLinkSensorVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1621, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1630, self, varargin{:});
     end
     function varargout = getRangeLinkVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1622, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1631, self, varargin{:});
     end
     function varargout = getRangeJointVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1623, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1632, self, varargin{:});
     end
     function varargout = getRangeDOFVariable(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1624, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1633, self, varargin{:});
     end
     function varargout = getDynamicVariablesOrdering(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1625, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1634, self, varargin{:});
     end
     function varargout = serializeDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1626, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1635, self, varargin{:});
     end
     function varargout = serializeSensorVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1627, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1636, self, varargin{:});
     end
     function varargout = serializeDynamicVariablesComputedFromFixedBaseRNEA(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1628, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1637, self, varargin{:});
     end
     function varargout = extractJointTorquesFromDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1629, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1638, self, varargin{:});
     end
     function varargout = extractLinkNetExternalWrenchesFromDynamicVariables(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1630, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1639, self, varargin{:});
     end
     function varargout = updateKinematicsFromFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1631, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1640, self, varargin{:});
     end
     function varargout = updateKinematicsFromFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1632, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1641, self, varargin{:});
     end
     function varargout = updateKinematicsFromTraversalFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1633, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1642, self, varargin{:});
     end
     function varargout = setNetExternalWrenchMeasurementFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1634, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1643, self, varargin{:});
     end
     function varargout = getNetExternalWrenchMeasurementFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1635, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1644, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1636, self);
+        iDynTreeMEX(1645, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m b/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
index 3eb7bff8fbd..289673c7db7 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdyOptions.m
@@ -9,7 +9,7 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1566, varargin{:});
+        tmp = iDynTreeMEX(1575, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
@@ -18,88 +18,88 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1567, self);
+        varargout{1} = iDynTreeMEX(1576, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1568, self, varargin{1});
+        iDynTreeMEX(1577, self, varargin{1});
       end
     end
     function varargout = includeAllNetExternalWrenchesAsDynamicVariables(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1569, self);
+        varargout{1} = iDynTreeMEX(1578, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1570, self, varargin{1});
+        iDynTreeMEX(1579, self, varargin{1});
       end
     end
     function varargout = includeAllJointAccelerationsAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1571, self);
+        varargout{1} = iDynTreeMEX(1580, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1572, self, varargin{1});
+        iDynTreeMEX(1581, self, varargin{1});
       end
     end
     function varargout = includeAllJointTorquesAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1573, self);
+        varargout{1} = iDynTreeMEX(1582, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1574, self, varargin{1});
+        iDynTreeMEX(1583, self, varargin{1});
       end
     end
     function varargout = includeAllNetExternalWrenchesAsSensors(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1575, self);
+        varargout{1} = iDynTreeMEX(1584, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1576, self, varargin{1});
+        iDynTreeMEX(1585, self, varargin{1});
       end
     end
     function varargout = includeFixedBaseExternalWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1577, self);
+        varargout{1} = iDynTreeMEX(1586, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1578, self, varargin{1});
+        iDynTreeMEX(1587, self, varargin{1});
       end
     end
     function varargout = jointOnWhichTheInternalWrenchIsMeasured(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1579, self);
+        varargout{1} = iDynTreeMEX(1588, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1580, self, varargin{1});
+        iDynTreeMEX(1589, self, varargin{1});
       end
     end
     function varargout = baseLink(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1581, self);
+        varargout{1} = iDynTreeMEX(1590, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1582, self, varargin{1});
+        iDynTreeMEX(1591, self, varargin{1});
       end
     end
     function varargout = checkConsistency(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1583, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1592, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1584, self);
+        iDynTreeMEX(1593, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m b/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
index 3967108b759..c99317a44f5 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdySensor.m
@@ -7,37 +7,37 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1585, self);
+        varargout{1} = iDynTreeMEX(1594, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1586, self, varargin{1});
+        iDynTreeMEX(1595, self, varargin{1});
       end
     end
     function varargout = id(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1587, self);
+        varargout{1} = iDynTreeMEX(1596, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1588, self, varargin{1});
+        iDynTreeMEX(1597, self, varargin{1});
       end
     end
     function varargout = range(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1589, self);
+        varargout{1} = iDynTreeMEX(1598, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1590, self, varargin{1});
+        iDynTreeMEX(1599, self, varargin{1});
       end
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1591, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1600, self, varargin{:});
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1592, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1601, self, varargin{:});
     end
     function self = BerdySensor(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -45,14 +45,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1593, varargin{:});
+        tmp = iDynTreeMEX(1602, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1594, self);
+        iDynTreeMEX(1603, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m b/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
index 44139b674ee..181291f47b9 100644
--- a/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
+++ b/bindings/matlab/autogenerated/+iDynTree/BerdySparseMAPSolver.m
@@ -9,58 +9,58 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1637, varargin{:});
+        tmp = iDynTreeMEX(1646, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1638, self);
+        iDynTreeMEX(1647, self);
         self.SwigClear();
       end
     end
     function varargout = setDynamicsConstraintsPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1639, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1648, self, varargin{:});
     end
     function varargout = setDynamicsRegularizationPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1640, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1649, self, varargin{:});
     end
     function varargout = setDynamicsRegularizationPriorExpectedValue(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1641, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1650, self, varargin{:});
     end
     function varargout = setMeasurementsPriorCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1642, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1651, self, varargin{:});
     end
     function varargout = dynamicsConstraintsPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1643, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1652, self, varargin{:});
     end
     function varargout = dynamicsRegularizationPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1644, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1653, self, varargin{:});
     end
     function varargout = dynamicsRegularizationPriorExpectedValue(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1645, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1654, self, varargin{:});
     end
     function varargout = measurementsPriorCovarianceInverse(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1646, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1655, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1647, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1656, self, varargin{:});
     end
     function varargout = initialize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1648, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1657, self, varargin{:});
     end
     function varargout = updateEstimateInformationFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1649, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1658, self, varargin{:});
     end
     function varargout = updateEstimateInformationFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1650, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1659, self, varargin{:});
     end
     function varargout = doEstimate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1651, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1660, self, varargin{:});
     end
     function varargout = getLastEstimate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1652, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1661, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/ColorViz.m b/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
index aa221b1ae43..371ddcce144 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ColorViz.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1881, self);
+        varargout{1} = iDynTreeMEX(1890, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1882, self, varargin{1});
+        iDynTreeMEX(1891, self, varargin{1});
       end
     end
     function varargout = g(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1883, self);
+        varargout{1} = iDynTreeMEX(1892, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1884, self, varargin{1});
+        iDynTreeMEX(1893, self, varargin{1});
       end
     end
     function varargout = b(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1885, self);
+        varargout{1} = iDynTreeMEX(1894, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1886, self, varargin{1});
+        iDynTreeMEX(1895, self, varargin{1});
       end
     end
     function varargout = a(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1887, self);
+        varargout{1} = iDynTreeMEX(1896, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1888, self, varargin{1});
+        iDynTreeMEX(1897, self, varargin{1});
       end
     end
     function self = ColorViz(varargin)
@@ -49,14 +49,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1889, varargin{:});
+        tmp = iDynTreeMEX(1898, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1890, self);
+        iDynTreeMEX(1899, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m b/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
index 09b666d94c1..7e52cdb686c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DiscreteExtendedKalmanFilterHelper.m
@@ -4,65 +4,65 @@
       this = iDynTreeMEX(3, self);
     end
     function varargout = ekf_f(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1704, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1713, self, varargin{:});
     end
     function varargout = ekf_h(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1705, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1714, self, varargin{:});
     end
     function varargout = ekfComputeJacobianF(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1706, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1715, self, varargin{:});
     end
     function varargout = ekfComputeJacobianH(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1707, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1716, self, varargin{:});
     end
     function varargout = ekfPredict(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1708, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1717, self, varargin{:});
     end
     function varargout = ekfUpdate(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1709, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1718, self, varargin{:});
     end
     function varargout = ekfInit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1710, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1719, self, varargin{:});
     end
     function varargout = ekfReset(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1711, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1720, self, varargin{:});
     end
     function varargout = ekfSetMeasurementVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1712, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1721, self, varargin{:});
     end
     function varargout = ekfSetInputVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1713, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1722, self, varargin{:});
     end
     function varargout = ekfSetInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1714, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1723, self, varargin{:});
     end
     function varargout = ekfSetStateCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1715, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1724, self, varargin{:});
     end
     function varargout = ekfSetSystemNoiseCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1716, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1725, self, varargin{:});
     end
     function varargout = ekfSetMeasurementNoiseCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1717, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1726, self, varargin{:});
     end
     function varargout = ekfSetStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1718, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1727, self, varargin{:});
     end
     function varargout = ekfSetInputSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1719, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1728, self, varargin{:});
     end
     function varargout = ekfSetOutputSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1720, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1729, self, varargin{:});
     end
     function varargout = ekfGetStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1721, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1730, self, varargin{:});
     end
     function varargout = ekfGetStateCovariance(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1722, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1731, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1723, self);
+        iDynTreeMEX(1732, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
index 19b99773f05..fc833796a05 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsComputations.m
@@ -9,118 +9,118 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1971, varargin{:});
+        tmp = iDynTreeMEX(1980, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1972, self);
+        iDynTreeMEX(1981, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1973, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1982, self, varargin{:});
     end
     function varargout = loadRobotModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1974, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1983, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1975, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1984, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1976, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1985, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1977, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1986, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1978, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1987, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1979, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1988, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1980, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1989, self, varargin{:});
     end
     function varargout = getFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1981, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1990, self, varargin{:});
     end
     function varargout = setFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1982, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1991, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1983, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1992, self, varargin{:});
     end
     function varargout = getWorldBaseTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1984, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1993, self, varargin{:});
     end
     function varargout = getBaseTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1985, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1994, self, varargin{:});
     end
     function varargout = getJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1986, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1995, self, varargin{:});
     end
     function varargout = getJointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1987, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1996, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1988, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1997, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1989, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1998, self, varargin{:});
     end
     function varargout = getWorldTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1990, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1999, self, varargin{:});
     end
     function varargout = getRelativeTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1991, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2000, self, varargin{:});
     end
     function varargout = getFrameTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1992, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2001, self, varargin{:});
     end
     function varargout = getFrameTwistInWorldOrient(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1993, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2002, self, varargin{:});
     end
     function varargout = getFrameProperSpatialAcceleration(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1994, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2003, self, varargin{:});
     end
     function varargout = getLinkIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1995, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2004, self, varargin{:});
     end
     function varargout = getLinkInertia(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1996, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2005, self, varargin{:});
     end
     function varargout = getJointIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1997, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2006, self, varargin{:});
     end
     function varargout = getJointName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1998, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2007, self, varargin{:});
     end
     function varargout = getJointLimits(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1999, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2008, self, varargin{:});
     end
     function varargout = inverseDynamics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2000, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2009, self, varargin{:});
     end
     function varargout = getFreeFloatingMassMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2001, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2010, self, varargin{:});
     end
     function varargout = getFrameJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2002, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2011, self, varargin{:});
     end
     function varargout = getDynamicsRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2003, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2012, self, varargin{:});
     end
     function varargout = getModelDynamicsParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2004, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2013, self, varargin{:});
     end
     function varargout = getCenterOfMass(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2005, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2014, self, varargin{:});
     end
     function varargout = getCenterOfMassJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(2006, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(2015, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
index f51a712ce43..7d9ca8ef733 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorGenerator.m
@@ -9,100 +9,100 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1792, varargin{:});
+        tmp = iDynTreeMEX(1801, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1793, self);
+        iDynTreeMEX(1802, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotAndSensorsModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1794, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1803, self, varargin{:});
     end
     function varargout = loadRobotAndSensorsModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1795, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1804, self, varargin{:});
     end
     function varargout = loadRegressorStructureFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1796, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1805, self, varargin{:});
     end
     function varargout = loadRegressorStructureFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1797, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1806, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1798, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1807, self, varargin{:});
     end
     function varargout = getNrOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1799, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1808, self, varargin{:});
     end
     function varargout = getNrOfOutputs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1800, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1809, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1801, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1810, self, varargin{:});
     end
     function varargout = getDescriptionOfParameter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1802, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1811, self, varargin{:});
     end
     function varargout = getDescriptionOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1803, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1812, self, varargin{:});
     end
     function varargout = getDescriptionOfOutput(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1804, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1813, self, varargin{:});
     end
     function varargout = getDescriptionOfOutputs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1805, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1814, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1806, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1815, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1807, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1816, self, varargin{:});
     end
     function varargout = getDescriptionOfLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1808, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1817, self, varargin{:});
     end
     function varargout = getDescriptionOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1809, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1818, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1810, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1819, self, varargin{:});
     end
     function varargout = getNrOfFakeLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1811, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1820, self, varargin{:});
     end
     function varargout = getBaseLinkName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1812, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1821, self, varargin{:});
     end
     function varargout = getSensorsModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1813, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1822, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1814, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1823, self, varargin{:});
     end
     function varargout = getSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1815, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1824, self, varargin{:});
     end
     function varargout = setTorqueSensorMeasurement(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1816, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1825, self, varargin{:});
     end
     function varargout = computeRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1817, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1826, self, varargin{:});
     end
     function varargout = getModelParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1818, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1827, self, varargin{:});
     end
     function varargout = computeFloatingBaseIdentifiableSubspace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1819, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1828, self, varargin{:});
     end
     function varargout = computeFixedBaseIdentifiableSubspace(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1820, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1829, self, varargin{:});
     end
     function varargout = generate_random_regressors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1821, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1830, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
index b5b76705269..6661f059828 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParameter.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1772, self);
+        varargout{1} = iDynTreeMEX(1781, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1773, self, varargin{1});
+        iDynTreeMEX(1782, self, varargin{1});
       end
     end
     function varargout = elemIndex(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1774, self);
+        varargout{1} = iDynTreeMEX(1783, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1775, self, varargin{1});
+        iDynTreeMEX(1784, self, varargin{1});
       end
     end
     function varargout = type(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1776, self);
+        varargout{1} = iDynTreeMEX(1785, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1777, self, varargin{1});
+        iDynTreeMEX(1786, self, varargin{1});
       end
     end
     function varargout = lt(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1778, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1787, self, varargin{:});
     end
     function varargout = eq(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1779, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1788, self, varargin{:});
     end
     function varargout = ne(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1780, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1789, self, varargin{:});
     end
     function self = DynamicsRegressorParameter(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -48,14 +48,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1781, varargin{:});
+        tmp = iDynTreeMEX(1790, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1782, self);
+        iDynTreeMEX(1791, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
index 23800572bdf..05f0d659e90 100644
--- a/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
+++ b/bindings/matlab/autogenerated/+iDynTree/DynamicsRegressorParametersList.m
@@ -7,26 +7,26 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1783, self);
+        varargout{1} = iDynTreeMEX(1792, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1784, self, varargin{1});
+        iDynTreeMEX(1793, self, varargin{1});
       end
     end
     function varargout = getDescriptionOfParameter(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1785, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1794, self, varargin{:});
     end
     function varargout = addParam(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1786, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1795, self, varargin{:});
     end
     function varargout = addList(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1787, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1796, self, varargin{:});
     end
     function varargout = findParam(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1788, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1797, self, varargin{:});
     end
     function varargout = getNrOfParameters(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1789, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1798, self, varargin{:});
     end
     function self = DynamicsRegressorParametersList(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
@@ -34,14 +34,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1790, varargin{:});
+        tmp = iDynTreeMEX(1799, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1791, self);
+        iDynTreeMEX(1800, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m b/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
index 06e4f8cd882..07e32130310 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ExtWrenchesAndJointTorquesEstimator.m
@@ -9,52 +9,52 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1537, varargin{:});
+        tmp = iDynTreeMEX(1546, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1538, self);
+        iDynTreeMEX(1547, self);
         self.SwigClear();
       end
     end
     function varargout = setModelAndSensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1539, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1548, self, varargin{:});
     end
     function varargout = loadModelAndSensorsFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1540, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1549, self, varargin{:});
     end
     function varargout = loadModelAndSensorsFromFileWithSpecifiedDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1541, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1550, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1542, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1551, self, varargin{:});
     end
     function varargout = sensors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1543, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1552, self, varargin{:});
     end
     function varargout = submodels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1544, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1553, self, varargin{:});
     end
     function varargout = updateKinematicsFromFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1545, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1554, self, varargin{:});
     end
     function varargout = updateKinematicsFromFixedBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1546, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1555, self, varargin{:});
     end
     function varargout = computeExpectedFTSensorsMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1547, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1556, self, varargin{:});
     end
     function varargout = estimateExtWrenchesAndJointTorques(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1548, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1557, self, varargin{:});
     end
     function varargout = checkThatTheModelIsStill(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1549, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1558, self, varargin{:});
     end
     function varargout = estimateLinkNetWrenchesWithoutGravity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1550, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1559, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m b/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
index 9da9181c363..48ba35be896 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IAttitudeEstimator.m
@@ -5,39 +5,39 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1661, self);
+        iDynTreeMEX(1670, self);
         self.SwigClear();
       end
     end
     function varargout = updateFilterWithMeasurements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1662, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1671, self, varargin{:});
     end
     function varargout = propagateStates(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1663, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1672, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRotationMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1664, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1673, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsQuaternion(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1665, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1674, self, varargin{:});
     end
     function varargout = getOrientationEstimateAsRPY(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1666, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1675, self, varargin{:});
     end
     function varargout = getInternalStateSize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1667, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1676, self, varargin{:});
     end
     function varargout = getInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1668, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1677, self, varargin{:});
     end
     function varargout = getDefaultInternalInitialState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1669, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1678, self, varargin{:});
     end
     function varargout = setInternalState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1670, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1679, self, varargin{:});
     end
     function varargout = setInternalStateInitialOrientation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1671, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1680, self, varargin{:});
     end
     function self = IAttitudeEstimator(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/ICamera.m b/bindings/matlab/autogenerated/+iDynTree/ICamera.m
index 82261667df3..c4ae036707f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ICamera.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ICamera.m
@@ -5,18 +5,18 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1877, self);
+        iDynTreeMEX(1886, self);
         self.SwigClear();
       end
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1878, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1887, self, varargin{:});
     end
     function varargout = setTarget(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1879, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1888, self, varargin{:});
     end
     function varargout = setUpVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1880, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1889, self, varargin{:});
     end
     function self = ICamera(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m b/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
index 3aba39646ae..aa9b465cec8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IEnvironment.m
@@ -5,33 +5,33 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1905, self);
+        iDynTreeMEX(1914, self);
         self.SwigClear();
       end
     end
     function varargout = getElements(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1906, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1915, self, varargin{:});
     end
     function varargout = setElementVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1907, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1916, self, varargin{:});
     end
     function varargout = setBackgroundColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1908, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1917, self, varargin{:});
     end
     function varargout = setAmbientLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1909, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1918, self, varargin{:});
     end
     function varargout = getLights(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1910, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1919, self, varargin{:});
     end
     function varargout = addLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1911, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1920, self, varargin{:});
     end
     function varargout = lightViz(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1912, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1921, self, varargin{:});
     end
     function varargout = removeLight(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1913, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1922, self, varargin{:});
     end
     function self = IEnvironment(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
index 4c6888b65cc..8603126f485 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IJetsVisualization.m
@@ -5,30 +5,30 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1914, self);
+        iDynTreeMEX(1923, self);
         self.SwigClear();
       end
     end
     function varargout = setJetsFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1915, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1924, self, varargin{:});
     end
     function varargout = getNrOfJets(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1916, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1925, self, varargin{:});
     end
     function varargout = getJetDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1917, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1926, self, varargin{:});
     end
     function varargout = setJetDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1918, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1927, self, varargin{:});
     end
     function varargout = setJetColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1919, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1928, self, varargin{:});
     end
     function varargout = setJetsDimensions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1920, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1929, self, varargin{:});
     end
     function varargout = setJetsIntensity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1921, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1930, self, varargin{:});
     end
     function self = IJetsVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/ILight.m b/bindings/matlab/autogenerated/+iDynTree/ILight.m
index 3746d5f21e7..fa792d390ba 100644
--- a/bindings/matlab/autogenerated/+iDynTree/ILight.m
+++ b/bindings/matlab/autogenerated/+iDynTree/ILight.m
@@ -5,48 +5,48 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1891, self);
+        iDynTreeMEX(1900, self);
         self.SwigClear();
       end
     end
     function varargout = getName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1892, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1901, self, varargin{:});
     end
     function varargout = setType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1893, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1902, self, varargin{:});
     end
     function varargout = getType(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1894, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1903, self, varargin{:});
     end
     function varargout = setPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1895, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1904, self, varargin{:});
     end
     function varargout = getPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1896, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1905, self, varargin{:});
     end
     function varargout = setDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1897, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1906, self, varargin{:});
     end
     function varargout = getDirection(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1898, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1907, self, varargin{:});
     end
     function varargout = setAmbientColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1899, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1908, self, varargin{:});
     end
     function varargout = getAmbientColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1900, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1909, self, varargin{:});
     end
     function varargout = setSpecularColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1901, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1910, self, varargin{:});
     end
     function varargout = getSpecularColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1902, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1911, self, varargin{:});
     end
     function varargout = setDiffuseColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1903, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1912, self, varargin{:});
     end
     function varargout = getDiffuseColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1904, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1913, self, varargin{:});
     end
     function self = ILight(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
index aa05b38a006..f5157cc0c25 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IModelVisualization.m
@@ -5,57 +5,57 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1929, self);
+        iDynTreeMEX(1938, self);
         self.SwigClear();
       end
     end
     function varargout = setPositions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1930, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1939, self, varargin{:});
     end
     function varargout = setLinkPositions(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1931, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1940, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1932, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1941, self, varargin{:});
     end
     function varargout = getInstanceName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1933, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1942, self, varargin{:});
     end
     function varargout = setModelVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1934, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1943, self, varargin{:});
     end
     function varargout = setModelColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1935, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1944, self, varargin{:});
     end
     function varargout = resetModelColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1936, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1945, self, varargin{:});
     end
     function varargout = setLinkColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1937, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1946, self, varargin{:});
     end
     function varargout = resetLinkColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1938, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1947, self, varargin{:});
     end
     function varargout = getLinkNames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1939, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1948, self, varargin{:});
     end
     function varargout = setLinkVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1940, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1949, self, varargin{:});
     end
     function varargout = getFeatures(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1941, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1950, self, varargin{:});
     end
     function varargout = setFeatureVisibility(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1942, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1951, self, varargin{:});
     end
     function varargout = jets(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1943, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1952, self, varargin{:});
     end
     function varargout = getWorldModelTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1944, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1953, self, varargin{:});
     end
     function varargout = getWorldLinkTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1945, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1954, self, varargin{:});
     end
     function self = IModelVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m b/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
index 10d0524834f..c84097f8fa0 100644
--- a/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
+++ b/bindings/matlab/autogenerated/+iDynTree/IVectorsVisualization.m
@@ -5,27 +5,27 @@
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1922, self);
+        iDynTreeMEX(1931, self);
         self.SwigClear();
       end
     end
     function varargout = addVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1923, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1932, self, varargin{:});
     end
     function varargout = getNrOfVectors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1924, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1933, self, varargin{:});
     end
     function varargout = getVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1925, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1934, self, varargin{:});
     end
     function varargout = updateVector(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1926, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1935, self, varargin{:});
     end
     function varargout = setVectorColor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1927, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1936, self, varargin{:});
     end
     function varargout = setVectorsAspect(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1928, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1937, self, varargin{:});
     end
     function self = IVectorsVisualization(varargin)
       if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
diff --git a/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m b/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
index 767cbff3371..fb0714876bd 100644
--- a/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
+++ b/bindings/matlab/autogenerated/+iDynTree/KinDynComputations.m
@@ -9,175 +9,175 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1822, varargin{:});
+        tmp = iDynTreeMEX(1831, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1823, self);
+        iDynTreeMEX(1832, self);
         self.SwigClear();
       end
     end
     function varargout = loadRobotModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1824, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1833, self, varargin{:});
     end
     function varargout = loadRobotModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1825, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1834, self, varargin{:});
     end
     function varargout = loadRobotModelFromString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1826, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1835, self, varargin{:});
     end
     function varargout = isValid(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1827, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1836, self, varargin{:});
     end
     function varargout = setFrameVelocityRepresentation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1828, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1837, self, varargin{:});
     end
     function varargout = getFrameVelocityRepresentation(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1829, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1838, self, varargin{:});
     end
     function varargout = getNrOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1830, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1839, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreeOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1831, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1840, self, varargin{:});
     end
     function varargout = getDescriptionOfDegreesOfFreedom(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1832, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1841, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1833, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1842, self, varargin{:});
     end
     function varargout = getNrOfFrames(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1834, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1843, self, varargin{:});
     end
     function varargout = getFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1835, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1844, self, varargin{:});
     end
     function varargout = setFloatingBase(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1836, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1845, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1837, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1846, self, varargin{:});
     end
     function varargout = getRobotModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1838, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1847, self, varargin{:});
     end
     function varargout = getRelativeJacobianSparsityPattern(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1839, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1848, self, varargin{:});
     end
     function varargout = getFrameFreeFloatingJacobianSparsityPattern(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1840, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1849, self, varargin{:});
     end
     function varargout = setJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1841, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1850, self, varargin{:});
     end
     function varargout = setRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1842, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1851, self, varargin{:});
     end
     function varargout = getRobotState(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1843, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1852, self, varargin{:});
     end
     function varargout = getWorldBaseTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1844, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1853, self, varargin{:});
     end
     function varargout = getBaseTwist(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1845, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1854, self, varargin{:});
     end
     function varargout = getJointPos(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1846, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1855, self, varargin{:});
     end
     function varargout = getJointVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1847, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1856, self, varargin{:});
     end
     function varargout = getModelVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1848, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1857, self, varargin{:});
     end
     function varargout = getFrameIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1849, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1858, self, varargin{:});
     end
     function varargout = getFrameName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1850, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1859, self, varargin{:});
     end
     function varargout = getWorldTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1851, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1860, self, varargin{:});
     end
     function varargout = getRelativeTransformExplicit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1852, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1861, self, varargin{:});
     end
     function varargout = getRelativeTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1853, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1862, self, varargin{:});
     end
     function varargout = getFrameVel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1854, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1863, self, varargin{:});
     end
     function varargout = getFrameAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1855, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1864, self, varargin{:});
     end
     function varargout = getFrameFreeFloatingJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1856, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1865, self, varargin{:});
     end
     function varargout = getRelativeJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1857, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1866, self, varargin{:});
     end
     function varargout = getRelativeJacobianExplicit(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1858, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1867, self, varargin{:});
     end
     function varargout = getFrameBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1859, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1868, self, varargin{:});
     end
     function varargout = getCenterOfMassPosition(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1860, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1869, self, varargin{:});
     end
     function varargout = getCenterOfMassVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1861, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1870, self, varargin{:});
     end
     function varargout = getCenterOfMassJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1862, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1871, self, varargin{:});
     end
     function varargout = getCenterOfMassBiasAcc(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1863, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1872, self, varargin{:});
     end
     function varargout = getAverageVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1864, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1873, self, varargin{:});
     end
     function varargout = getAverageVelocityJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1865, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1874, self, varargin{:});
     end
     function varargout = getCentroidalAverageVelocity(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1866, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1875, self, varargin{:});
     end
     function varargout = getCentroidalAverageVelocityJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1867, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1876, self, varargin{:});
     end
     function varargout = getLinearAngularMomentum(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1868, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1877, self, varargin{:});
     end
     function varargout = getLinearAngularMomentumJacobian(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1869, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1878, self, varargin{:});
     end
     function varargout = getCentroidalTotalMomentum(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1870, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1879, self, varargin{:});
     end
     function varargout = getFreeFloatingMassMatrix(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1871, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1880, self, varargin{:});
     end
     function varargout = inverseDynamics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1872, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1881, self, varargin{:});
     end
     function varargout = generalizedBiasForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1873, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1882, self, varargin{:});
     end
     function varargout = generalizedGravityForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1874, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1883, self, varargin{:});
     end
     function varargout = generalizedExternalForces(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1875, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1884, self, varargin{:});
     end
     function varargout = inverseDynamicsInertialParametersRegressor(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1876, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1885, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m b/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
index 647fc6b8a3a..600f6f440b8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
+++ b/bindings/matlab/autogenerated/+iDynTree/LinkUnknownWrenchContacts.m
@@ -9,41 +9,41 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1502, varargin{:});
+        tmp = iDynTreeMEX(1511, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = clear(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1503, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1512, self, varargin{:});
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1504, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1513, self, varargin{:});
     end
     function varargout = getNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1505, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1514, self, varargin{:});
     end
     function varargout = setNrOfContactsForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1506, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1515, self, varargin{:});
     end
     function varargout = addNewContactForLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1507, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1516, self, varargin{:});
     end
     function varargout = addNewContactInFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1508, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1517, self, varargin{:});
     end
     function varargout = addNewUnknownFullWrenchInFrameOrigin(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1509, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1518, self, varargin{:});
     end
     function varargout = contactWrench(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1510, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1519, self, varargin{:});
     end
     function varargout = toString(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1511, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1520, self, varargin{:});
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1512, self);
+        iDynTreeMEX(1521, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/ModelCalibrationHelper.m b/bindings/matlab/autogenerated/+iDynTree/ModelCalibrationHelper.m
new file mode 100644
index 00000000000..4a98d9bd03d
--- /dev/null
+++ b/bindings/matlab/autogenerated/+iDynTree/ModelCalibrationHelper.m
@@ -0,0 +1,47 @@
+classdef ModelCalibrationHelper < SwigRef
+  methods
+    function this = swig_this(self)
+      this = iDynTreeMEX(3, self);
+    end
+    function self = ModelCalibrationHelper(varargin)
+      if nargin==1 && strcmp(class(varargin{1}),'SwigRef')
+        if ~isnull(varargin{1})
+          self.swigPtr = varargin{1}.swigPtr;
+        end
+      else
+        tmp = iDynTreeMEX(1490, varargin{:});
+        self.swigPtr = tmp.swigPtr;
+        tmp.SwigClear();
+      end
+    end
+    function delete(self)
+      if self.swigPtr
+        iDynTreeMEX(1491, self);
+        self.SwigClear();
+      end
+    end
+    function varargout = loadModelFromString(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1492, self, varargin{:});
+    end
+    function varargout = loadModelFromFile(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1493, self, varargin{:});
+    end
+    function varargout = updateModelInertialParametersToString(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1494, self, varargin{:});
+    end
+    function varargout = updateModelInertialParametersToFile(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1495, self, varargin{:});
+    end
+    function varargout = model(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1496, self, varargin{:});
+    end
+    function varargout = sensors(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1497, self, varargin{:});
+    end
+    function varargout = isValid(self,varargin)
+      [varargout{1:nargout}] = iDynTreeMEX(1498, self, varargin{:});
+    end
+  end
+  methods(Static)
+  end
+end
diff --git a/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m b/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
index fcb310a459b..40aaa592ec3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
+++ b/bindings/matlab/autogenerated/+iDynTree/SimpleLeggedOdometry.m
@@ -9,46 +9,46 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1551, varargin{:});
+        tmp = iDynTreeMEX(1560, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1552, self);
+        iDynTreeMEX(1561, self);
         self.SwigClear();
       end
     end
     function varargout = setModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1553, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1562, self, varargin{:});
     end
     function varargout = loadModelFromFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1554, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1563, self, varargin{:});
     end
     function varargout = loadModelFromFileWithSpecifiedDOFs(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1555, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1564, self, varargin{:});
     end
     function varargout = model(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1556, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1565, self, varargin{:});
     end
     function varargout = updateKinematics(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1557, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1566, self, varargin{:});
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1558, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1567, self, varargin{:});
     end
     function varargout = changeFixedFrame(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1559, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1568, self, varargin{:});
     end
     function varargout = getCurrentFixedLink(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1560, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1569, self, varargin{:});
     end
     function varargout = getWorldLinkTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1561, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1570, self, varargin{:});
     end
     function varargout = getWorldFrameTransform(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1562, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1571, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m b/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
index 89eaba4eec3..68a15760277 100644
--- a/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
+++ b/bindings/matlab/autogenerated/+iDynTree/UnknownWrenchContact.m
@@ -9,7 +9,7 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1490, varargin{:});
+        tmp = iDynTreeMEX(1499, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
@@ -18,55 +18,55 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1491, self);
+        varargout{1} = iDynTreeMEX(1500, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1492, self, varargin{1});
+        iDynTreeMEX(1501, self, varargin{1});
       end
     end
     function varargout = contactPoint(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1493, self);
+        varargout{1} = iDynTreeMEX(1502, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1494, self, varargin{1});
+        iDynTreeMEX(1503, self, varargin{1});
       end
     end
     function varargout = forceDirection(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1495, self);
+        varargout{1} = iDynTreeMEX(1504, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1496, self, varargin{1});
+        iDynTreeMEX(1505, self, varargin{1});
       end
     end
     function varargout = knownWrench(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1497, self);
+        varargout{1} = iDynTreeMEX(1506, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1498, self, varargin{1});
+        iDynTreeMEX(1507, self, varargin{1});
       end
     end
     function varargout = contactId(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1499, self);
+        varargout{1} = iDynTreeMEX(1508, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1500, self, varargin{1});
+        iDynTreeMEX(1509, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1501, self);
+        iDynTreeMEX(1510, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/Visualizer.m b/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
index 12ace447c7e..d5fd29c6e4f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/Visualizer.m
@@ -9,55 +9,55 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1956, varargin{:});
+        tmp = iDynTreeMEX(1965, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1957, self);
+        iDynTreeMEX(1966, self);
         self.SwigClear();
       end
     end
     function varargout = init(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1958, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1967, self, varargin{:});
     end
     function varargout = getNrOfVisualizedModels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1959, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1968, self, varargin{:});
     end
     function varargout = getModelInstanceName(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1960, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1969, self, varargin{:});
     end
     function varargout = getModelInstanceIndex(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1961, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1970, self, varargin{:});
     end
     function varargout = addModel(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1962, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1971, self, varargin{:});
     end
     function varargout = modelViz(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1963, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1972, self, varargin{:});
     end
     function varargout = camera(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1964, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1973, self, varargin{:});
     end
     function varargout = enviroment(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1965, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1974, self, varargin{:});
     end
     function varargout = vectors(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1966, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1975, self, varargin{:});
     end
     function varargout = run(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1967, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1976, self, varargin{:});
     end
     function varargout = draw(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1968, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1977, self, varargin{:});
     end
     function varargout = drawToFile(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1969, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1978, self, varargin{:});
     end
     function varargout = close(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1970, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1979, self, varargin{:});
     end
   end
   methods(Static)
diff --git a/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m b/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
index e756db5a577..e1d0fe77cc8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/VisualizerOptions.m
@@ -7,40 +7,40 @@
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1946, self);
+        varargout{1} = iDynTreeMEX(1955, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1947, self, varargin{1});
+        iDynTreeMEX(1956, self, varargin{1});
       end
     end
     function varargout = winWidth(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1948, self);
+        varargout{1} = iDynTreeMEX(1957, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1949, self, varargin{1});
+        iDynTreeMEX(1958, self, varargin{1});
       end
     end
     function varargout = winHeight(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1950, self);
+        varargout{1} = iDynTreeMEX(1959, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1951, self, varargin{1});
+        iDynTreeMEX(1960, self, varargin{1});
       end
     end
     function varargout = rootFrameArrowsDimension(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1952, self);
+        varargout{1} = iDynTreeMEX(1961, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1953, self, varargin{1});
+        iDynTreeMEX(1962, self, varargin{1});
       end
     end
     function self = VisualizerOptions(varargin)
@@ -49,14 +49,14 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1954, varargin{:});
+        tmp = iDynTreeMEX(1963, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1955, self);
+        iDynTreeMEX(1964, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m b/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
index 71dff558717..6ff39582264 100644
--- a/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
+++ b/bindings/matlab/autogenerated/+iDynTree/computeLinkNetWrenchesWithoutGravity.m
@@ -1,3 +1,3 @@
 function varargout = computeLinkNetWrenchesWithoutGravity(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1535, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1544, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
index da259fdf660..23976b6a4bf 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelAccKinematics.m
@@ -1,3 +1,3 @@
 function varargout = dynamicsEstimationForwardVelAccKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1533, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1542, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
index c301eba8cce..b279ad6f48f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
+++ b/bindings/matlab/autogenerated/+iDynTree/dynamicsEstimationForwardVelKinematics.m
@@ -1,3 +1,3 @@
 function varargout = dynamicsEstimationForwardVelKinematics(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1534, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1543, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
index 0849c07d164..30a13511e8f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenches.m
@@ -1,3 +1,3 @@
 function varargout = estimateExternalWrenches(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1532, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1541, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
index e29c2a0fcd6..82610133ff4 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesBuffers.m
@@ -9,86 +9,86 @@
           self.swigPtr = varargin{1}.swigPtr;
         end
       else
-        tmp = iDynTreeMEX(1513, varargin{:});
+        tmp = iDynTreeMEX(1522, varargin{:});
         self.swigPtr = tmp.swigPtr;
         tmp.SwigClear();
       end
     end
     function varargout = resize(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1514, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1523, self, varargin{:});
     end
     function varargout = getNrOfSubModels(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1515, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1524, self, varargin{:});
     end
     function varargout = getNrOfLinks(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1516, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1525, self, varargin{:});
     end
     function varargout = isConsistent(self,varargin)
-      [varargout{1:nargout}] = iDynTreeMEX(1517, self, varargin{:});
+      [varargout{1:nargout}] = iDynTreeMEX(1526, self, varargin{:});
     end
     function varargout = A(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1518, self);
+        varargout{1} = iDynTreeMEX(1527, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1519, self, varargin{1});
+        iDynTreeMEX(1528, self, varargin{1});
       end
     end
     function varargout = x(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1520, self);
+        varargout{1} = iDynTreeMEX(1529, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1521, self, varargin{1});
+        iDynTreeMEX(1530, self, varargin{1});
       end
     end
     function varargout = b(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1522, self);
+        varargout{1} = iDynTreeMEX(1531, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1523, self, varargin{1});
+        iDynTreeMEX(1532, self, varargin{1});
       end
     end
     function varargout = pinvA(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1524, self);
+        varargout{1} = iDynTreeMEX(1533, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1525, self, varargin{1});
+        iDynTreeMEX(1534, self, varargin{1});
       end
     end
     function varargout = b_contacts_subtree(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1526, self);
+        varargout{1} = iDynTreeMEX(1535, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1527, self, varargin{1});
+        iDynTreeMEX(1536, self, varargin{1});
       end
     end
     function varargout = subModelBase_H_link(self, varargin)
       narginchk(1, 2)
       if nargin==1
         nargoutchk(0, 1)
-        varargout{1} = iDynTreeMEX(1528, self);
+        varargout{1} = iDynTreeMEX(1537, self);
       else
         nargoutchk(0, 0)
-        iDynTreeMEX(1529, self, varargin{1});
+        iDynTreeMEX(1538, self, varargin{1});
       end
     end
     function delete(self)
       if self.swigPtr
-        iDynTreeMEX(1530, self);
+        iDynTreeMEX(1539, self);
         self.SwigClear();
       end
     end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
index 15f214467cf..6d538b372d8 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateExternalWrenchesWithoutInternalFT.m
@@ -1,3 +1,3 @@
 function varargout = estimateExternalWrenchesWithoutInternalFT(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1531, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1540, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m b/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m
index f40cfe8d0c3..12b99e0b857 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateInertialParametersFromLinkBoundingBoxesAndTotalMass.m
@@ -1,3 +1,3 @@
 function varargout = estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1771, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1780, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m b/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
index 40b419ddbe1..49c2770b41a 100644
--- a/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
+++ b/bindings/matlab/autogenerated/+iDynTree/estimateLinkContactWrenchesFromLinkNetExternalWrenches.m
@@ -1,3 +1,3 @@
 function varargout = estimateLinkContactWrenchesFromLinkNetExternalWrenches(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1536, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1545, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m b/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
index 16d7ce70d84..4c1ce3c6501 100644
--- a/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
+++ b/bindings/matlab/autogenerated/+iDynTree/input_dimensions.m
@@ -1,3 +1,3 @@
 function v = input_dimensions()
-  v = iDynTreeMEX(1726);
+  v = iDynTreeMEX(1735);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
index 6eb5622f355..9f832ffac91 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isDOFBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isDOFBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1565, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1574, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
index 68b28970e6c..1da4d8eea3c 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isJointBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isJointBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1564, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1573, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m b/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
index ca1ec7e7207..51692e816a3 100644
--- a/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
+++ b/bindings/matlab/autogenerated/+iDynTree/isLinkBerdyDynamicVariable.m
@@ -1,3 +1,3 @@
 function varargout = isLinkBerdyDynamicVariable(varargin)
-  [varargout{1:nargout}] = iDynTreeMEX(1563, varargin{:});
+  [varargout{1:nargout}] = iDynTreeMEX(1572, varargin{:});
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
index 0b0c0b44d2f..5b6dbc4c35f 100644
--- a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_with_magnetometer.m
@@ -1,3 +1,3 @@
 function v = output_dimensions_with_magnetometer()
-  v = iDynTreeMEX(1724);
+  v = iDynTreeMEX(1733);
 end
diff --git a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
index 375a71cd363..b9864d7bdbe 100644
--- a/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
+++ b/bindings/matlab/autogenerated/+iDynTree/output_dimensions_without_magnetometer.m
@@ -1,3 +1,3 @@
 function v = output_dimensions_without_magnetometer()
-  v = iDynTreeMEX(1725);
+  v = iDynTreeMEX(1734);
 end
diff --git a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
index 52394cc4d0f..7b709e91fec 100644
--- a/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
+++ b/bindings/matlab/autogenerated/iDynTreeMATLAB_wrap.cxx
@@ -1325,108 +1325,109 @@ namespace swig {
 #define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_1_t swig_types[113]
 #define SWIGTYPE_p_iDynTree__MatrixFixSizeT_6_6_t swig_types[114]
 #define SWIGTYPE_p_iDynTree__Model swig_types[115]
-#define SWIGTYPE_p_iDynTree__ModelExporter swig_types[116]
-#define SWIGTYPE_p_iDynTree__ModelExporterOptions swig_types[117]
-#define SWIGTYPE_p_iDynTree__ModelLoader swig_types[118]
-#define SWIGTYPE_p_iDynTree__ModelParserOptions swig_types[119]
-#define SWIGTYPE_p_iDynTree__ModelSolidShapes swig_types[120]
-#define SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian swig_types[121]
-#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t swig_types[122]
-#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t swig_types[123]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t swig_types[124]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t swig_types[125]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t swig_types[126]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t swig_types[127]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t swig_types[128]
-#define SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t swig_types[129]
-#define SWIGTYPE_p_iDynTree__Neighbor swig_types[130]
-#define SWIGTYPE_p_iDynTree__Position swig_types[131]
-#define SWIGTYPE_p_iDynTree__PositionRaw swig_types[132]
-#define SWIGTYPE_p_iDynTree__PositionSemantics swig_types[133]
-#define SWIGTYPE_p_iDynTree__PrismaticJoint swig_types[134]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorGenerator swig_types[135]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParameter swig_types[136]
-#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParametersList swig_types[137]
-#define SWIGTYPE_p_iDynTree__RevoluteJoint swig_types[138]
-#define SWIGTYPE_p_iDynTree__RigidBodyInertiaNonLinearParametrization swig_types[139]
-#define SWIGTYPE_p_iDynTree__Rotation swig_types[140]
-#define SWIGTYPE_p_iDynTree__RotationRaw swig_types[141]
-#define SWIGTYPE_p_iDynTree__RotationSemantics swig_types[142]
-#define SWIGTYPE_p_iDynTree__RotationalInertiaRaw swig_types[143]
-#define SWIGTYPE_p_iDynTree__Sensor swig_types[144]
-#define SWIGTYPE_p_iDynTree__SensorsList swig_types[145]
-#define SWIGTYPE_p_iDynTree__SensorsMeasurements swig_types[146]
-#define SWIGTYPE_p_iDynTree__SimpleLeggedOdometry swig_types[147]
-#define SWIGTYPE_p_iDynTree__SixAxisForceTorqueSensor swig_types[148]
-#define SWIGTYPE_p_iDynTree__SolidShape swig_types[149]
-#define SWIGTYPE_p_iDynTree__SpanT_double__1_t swig_types[150]
-#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t swig_types[151]
-#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t swig_types[152]
-#define SWIGTYPE_p_iDynTree__SpatialAcc swig_types[153]
-#define SWIGTYPE_p_iDynTree__SpatialForceVector swig_types[154]
-#define SWIGTYPE_p_iDynTree__SpatialInertia swig_types[155]
-#define SWIGTYPE_p_iDynTree__SpatialInertiaRaw swig_types[156]
-#define SWIGTYPE_p_iDynTree__SpatialMomentum swig_types[157]
-#define SWIGTYPE_p_iDynTree__SpatialMotionVector swig_types[158]
-#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearForceVector3Semantics_iDynTree__AngularForceVector3Semantics_t swig_types[159]
-#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearMotionVector3Semantics_iDynTree__AngularMotionVector3Semantics_t swig_types[160]
-#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t swig_types[161]
-#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t swig_types[162]
-#define SWIGTYPE_p_iDynTree__Sphere swig_types[163]
-#define SWIGTYPE_p_iDynTree__SubModelDecomposition swig_types[164]
-#define SWIGTYPE_p_iDynTree__ThreeAxisAngularAccelerometerSensor swig_types[165]
-#define SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor swig_types[166]
-#define SWIGTYPE_p_iDynTree__Transform swig_types[167]
-#define SWIGTYPE_p_iDynTree__TransformDerivative swig_types[168]
-#define SWIGTYPE_p_iDynTree__TransformSemantics swig_types[169]
-#define SWIGTYPE_p_iDynTree__Traversal swig_types[170]
-#define SWIGTYPE_p_iDynTree__Triplets swig_types[171]
-#define SWIGTYPE_p_iDynTree__Twist swig_types[172]
-#define SWIGTYPE_p_iDynTree__URDFParserOptions swig_types[173]
-#define SWIGTYPE_p_iDynTree__UnknownWrenchContact swig_types[174]
-#define SWIGTYPE_p_iDynTree__VectorDynSize swig_types[175]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_10_t swig_types[176]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_16_t swig_types[177]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_2_t swig_types[178]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t swig_types[179]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t swig_types[180]
-#define SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t swig_types[181]
-#define SWIGTYPE_p_iDynTree__Visualizer swig_types[182]
-#define SWIGTYPE_p_iDynTree__VisualizerOptions swig_types[183]
-#define SWIGTYPE_p_iDynTree__Wrench swig_types[184]
-#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t swig_types[185]
-#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t swig_types[186]
-#define SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers swig_types[187]
-#define SWIGTYPE_p_index_type swig_types[188]
-#define SWIGTYPE_p_int swig_types[189]
-#define SWIGTYPE_p_iterator swig_types[190]
-#define SWIGTYPE_p_pointer swig_types[191]
-#define SWIGTYPE_p_reference swig_types[192]
-#define SWIGTYPE_p_reverse_iterator swig_types[193]
-#define SWIGTYPE_p_size_type swig_types[194]
-#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t swig_types[195]
-#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdySensor_t swig_types[196]
-#define SWIGTYPE_p_std__allocatorT_std__string_t swig_types[197]
-#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t swig_types[198]
-#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t swig_types[199]
-#define SWIGTYPE_p_std__string swig_types[200]
-#define SWIGTYPE_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t swig_types[201]
-#define SWIGTYPE_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t swig_types[202]
-#define SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t swig_types[203]
-#define SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t swig_types[204]
-#define SWIGTYPE_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t swig_types[205]
-#define SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t swig_types[206]
-#define SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t swig_types[207]
-#define SWIGTYPE_p_std__vectorT_int_std__allocatorT_int_t_t swig_types[208]
-#define SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t swig_types[209]
-#define SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t swig_types[210]
-#define SWIGTYPE_p_swig__MatlabSwigIterator swig_types[211]
-#define SWIGTYPE_p_typed_iterator swig_types[212]
-#define SWIGTYPE_p_unsigned_int swig_types[213]
-#define SWIGTYPE_p_unsigned_long swig_types[214]
-#define SWIGTYPE_p_value_type swig_types[215]
-static swig_type_info *swig_types[217];
-static swig_module_info swig_module = {swig_types, 216, 0, 0, 0, 0};
+#define SWIGTYPE_p_iDynTree__ModelCalibrationHelper swig_types[116]
+#define SWIGTYPE_p_iDynTree__ModelExporter swig_types[117]
+#define SWIGTYPE_p_iDynTree__ModelExporterOptions swig_types[118]
+#define SWIGTYPE_p_iDynTree__ModelLoader swig_types[119]
+#define SWIGTYPE_p_iDynTree__ModelParserOptions swig_types[120]
+#define SWIGTYPE_p_iDynTree__ModelSolidShapes swig_types[121]
+#define SWIGTYPE_p_iDynTree__MomentumFreeFloatingJacobian swig_types[122]
+#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__AngularMotionVector3_t swig_types[123]
+#define SWIGTYPE_p_iDynTree__MotionVector3T_iDynTree__LinearMotionVector3_t swig_types[124]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_1_1_t swig_types[125]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_2_2_t swig_types[126]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_3_3_t swig_types[127]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_4_4_t swig_types[128]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_5_5_t swig_types[129]
+#define SWIGTYPE_p_iDynTree__MovableJointImplT_6_6_t swig_types[130]
+#define SWIGTYPE_p_iDynTree__Neighbor swig_types[131]
+#define SWIGTYPE_p_iDynTree__Position swig_types[132]
+#define SWIGTYPE_p_iDynTree__PositionRaw swig_types[133]
+#define SWIGTYPE_p_iDynTree__PositionSemantics swig_types[134]
+#define SWIGTYPE_p_iDynTree__PrismaticJoint swig_types[135]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorGenerator swig_types[136]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParameter swig_types[137]
+#define SWIGTYPE_p_iDynTree__Regressors__DynamicsRegressorParametersList swig_types[138]
+#define SWIGTYPE_p_iDynTree__RevoluteJoint swig_types[139]
+#define SWIGTYPE_p_iDynTree__RigidBodyInertiaNonLinearParametrization swig_types[140]
+#define SWIGTYPE_p_iDynTree__Rotation swig_types[141]
+#define SWIGTYPE_p_iDynTree__RotationRaw swig_types[142]
+#define SWIGTYPE_p_iDynTree__RotationSemantics swig_types[143]
+#define SWIGTYPE_p_iDynTree__RotationalInertiaRaw swig_types[144]
+#define SWIGTYPE_p_iDynTree__Sensor swig_types[145]
+#define SWIGTYPE_p_iDynTree__SensorsList swig_types[146]
+#define SWIGTYPE_p_iDynTree__SensorsMeasurements swig_types[147]
+#define SWIGTYPE_p_iDynTree__SimpleLeggedOdometry swig_types[148]
+#define SWIGTYPE_p_iDynTree__SixAxisForceTorqueSensor swig_types[149]
+#define SWIGTYPE_p_iDynTree__SolidShape swig_types[150]
+#define SWIGTYPE_p_iDynTree__SpanT_double__1_t swig_types[151]
+#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__ColumnMajor_t swig_types[152]
+#define SWIGTYPE_p_iDynTree__SparseMatrixT_iDynTree__RowMajor_t swig_types[153]
+#define SWIGTYPE_p_iDynTree__SpatialAcc swig_types[154]
+#define SWIGTYPE_p_iDynTree__SpatialForceVector swig_types[155]
+#define SWIGTYPE_p_iDynTree__SpatialInertia swig_types[156]
+#define SWIGTYPE_p_iDynTree__SpatialInertiaRaw swig_types[157]
+#define SWIGTYPE_p_iDynTree__SpatialMomentum swig_types[158]
+#define SWIGTYPE_p_iDynTree__SpatialMotionVector swig_types[159]
+#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearForceVector3Semantics_iDynTree__AngularForceVector3Semantics_t swig_types[160]
+#define SWIGTYPE_p_iDynTree__SpatialVectorSemanticsT_iDynTree__LinearMotionVector3Semantics_iDynTree__AngularMotionVector3Semantics_t swig_types[161]
+#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialForceVector_t swig_types[162]
+#define SWIGTYPE_p_iDynTree__SpatialVectorT_iDynTree__SpatialMotionVector_t swig_types[163]
+#define SWIGTYPE_p_iDynTree__Sphere swig_types[164]
+#define SWIGTYPE_p_iDynTree__SubModelDecomposition swig_types[165]
+#define SWIGTYPE_p_iDynTree__ThreeAxisAngularAccelerometerSensor swig_types[166]
+#define SWIGTYPE_p_iDynTree__ThreeAxisForceTorqueContactSensor swig_types[167]
+#define SWIGTYPE_p_iDynTree__Transform swig_types[168]
+#define SWIGTYPE_p_iDynTree__TransformDerivative swig_types[169]
+#define SWIGTYPE_p_iDynTree__TransformSemantics swig_types[170]
+#define SWIGTYPE_p_iDynTree__Traversal swig_types[171]
+#define SWIGTYPE_p_iDynTree__Triplets swig_types[172]
+#define SWIGTYPE_p_iDynTree__Twist swig_types[173]
+#define SWIGTYPE_p_iDynTree__URDFParserOptions swig_types[174]
+#define SWIGTYPE_p_iDynTree__UnknownWrenchContact swig_types[175]
+#define SWIGTYPE_p_iDynTree__VectorDynSize swig_types[176]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_10_t swig_types[177]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_16_t swig_types[178]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_2_t swig_types[179]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_3_t swig_types[180]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_4_t swig_types[181]
+#define SWIGTYPE_p_iDynTree__VectorFixSizeT_6_t swig_types[182]
+#define SWIGTYPE_p_iDynTree__Visualizer swig_types[183]
+#define SWIGTYPE_p_iDynTree__VisualizerOptions swig_types[184]
+#define SWIGTYPE_p_iDynTree__Wrench swig_types[185]
+#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t swig_types[186]
+#define SWIGTYPE_p_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t swig_types[187]
+#define SWIGTYPE_p_iDynTree__estimateExternalWrenchesBuffers swig_types[188]
+#define SWIGTYPE_p_index_type swig_types[189]
+#define SWIGTYPE_p_int swig_types[190]
+#define SWIGTYPE_p_iterator swig_types[191]
+#define SWIGTYPE_p_pointer swig_types[192]
+#define SWIGTYPE_p_reference swig_types[193]
+#define SWIGTYPE_p_reverse_iterator swig_types[194]
+#define SWIGTYPE_p_size_type swig_types[195]
+#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdyDynamicVariable_t swig_types[196]
+#define SWIGTYPE_p_std__allocatorT_iDynTree__BerdySensor_t swig_types[197]
+#define SWIGTYPE_p_std__allocatorT_std__string_t swig_types[198]
+#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_false_t_t swig_types[199]
+#define SWIGTYPE_p_std__reverse_iteratorT_iDynTree__details__span_iteratorT_iDynTree__SpanT_double__1_t_true_t_t swig_types[200]
+#define SWIGTYPE_p_std__string swig_types[201]
+#define SWIGTYPE_p_std__vectorT_iDynTree__BerdyDynamicVariable_std__allocatorT_iDynTree__BerdyDynamicVariable_t_t swig_types[202]
+#define SWIGTYPE_p_std__vectorT_iDynTree__BerdySensor_std__allocatorT_iDynTree__BerdySensor_t_t swig_types[203]
+#define SWIGTYPE_p_std__vectorT_iDynTree__MatrixDynSize_std__allocatorT_iDynTree__MatrixDynSize_t_t swig_types[204]
+#define SWIGTYPE_p_std__vectorT_iDynTree__Position_std__allocatorT_iDynTree__Position_t_t swig_types[205]
+#define SWIGTYPE_p_std__vectorT_iDynTree__Regressors__DynamicsRegressorParameter_std__allocatorT_iDynTree__Regressors__DynamicsRegressorParameter_t_t swig_types[206]
+#define SWIGTYPE_p_std__vectorT_iDynTree__VectorDynSize_std__allocatorT_iDynTree__VectorDynSize_t_t swig_types[207]
+#define SWIGTYPE_p_std__vectorT_iDynTree__VectorFixSizeT_6_t_std__allocatorT_iDynTree__VectorFixSizeT_6_t_t_t swig_types[208]
+#define SWIGTYPE_p_std__vectorT_int_std__allocatorT_int_t_t swig_types[209]
+#define SWIGTYPE_p_std__vectorT_std__string_std__allocatorT_std__string_t_t swig_types[210]
+#define SWIGTYPE_p_std__vectorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_std__allocatorT_std__vectorT_iDynTree__SolidShape_p_std__allocatorT_iDynTree__SolidShape_p_t_t_t_t swig_types[211]
+#define SWIGTYPE_p_swig__MatlabSwigIterator swig_types[212]
+#define SWIGTYPE_p_typed_iterator swig_types[213]
+#define SWIGTYPE_p_unsigned_int swig_types[214]
+#define SWIGTYPE_p_unsigned_long swig_types[215]
+#define SWIGTYPE_p_value_type swig_types[216]
+static swig_type_info *swig_types[218];
+static swig_module_info swig_module = {swig_types, 217, 0, 0, 0, 0};
 #define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name)
 #define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name)
 
@@ -2983,6 +2984,8 @@ SWIGINTERN void std_vector_Sl_std_string_Sg__insert__SWIG_1(std::vector< std::st
 #include "iDynTree/ModelIO/URDFGenericSensorsImport.h"
 #include "iDynTree/ModelIO/ModelLoader.h"
 #include "iDynTree/ModelIO/ModelExporter.h"
+#include "iDynTree/ModelIO/ModelCalibrationHelper.h"
+
 
 // Estimation related classes
 #include "iDynTree/Estimation/ExternalWrenchesEstimation.h"
@@ -71287,6 +71290,903 @@ int _wrap_ModelExporter_exportModelToFile(int resc, mxArray *resv[], int argc, m
 }
 
 
+int _wrap_new_ModelCalibrationHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  mxArray * _out;
+  iDynTree::ModelCalibrationHelper *result = 0 ;
+  
+  if (!SWIG_check_num_args("new_ModelCalibrationHelper",argc,0,0,0)) {
+    SWIG_fail;
+  }
+  (void)argv;
+  result = (iDynTree::ModelCalibrationHelper *)new iDynTree::ModelCalibrationHelper();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 1 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_delete_ModelCalibrationHelper(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  
+  int is_owned;
+  if (!SWIG_check_num_args("delete_ModelCalibrationHelper",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  is_owned = SWIG_Matlab_isOwned(argv[0]);
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, SWIG_POINTER_DISOWN |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_ModelCalibrationHelper" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  if (is_owned) {
+    delete arg1;
+  }
+  _out = (mxArray*)0;
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_loadModelFromString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_loadModelFromString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromString((std::string const &)*arg2,(std::string const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_loadModelFromString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_loadModelFromString",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_loadModelFromString" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromString((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_loadModelFromString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelCalibrationHelper_loadModelFromString__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelCalibrationHelper_loadModelFromString__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelCalibrationHelper_loadModelFromString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelCalibrationHelper::loadModelFromString(std::string const &,std::string const &)\n"
+    "    iDynTree::ModelCalibrationHelper::loadModelFromString(std::string const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_loadModelFromFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  std::string *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  int res3 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_loadModelFromFile",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  {
+    std::string *ptr = (std::string *)0;
+    res3 = SWIG_AsPtr_std_string(argv[2], &ptr);
+    if (!SWIG_IsOK(res3)) {
+      SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "3"" of type '" "std::string const &""'"); 
+    }
+    arg3 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromFile((std::string const &)*arg2,(std::string const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  if (SWIG_IsNewObj(res3)) delete arg3;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_loadModelFromFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_loadModelFromFile",argc,2,2,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_loadModelFromFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  result = (bool)(arg1)->loadModelFromFile((std::string const &)*arg2);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_loadModelFromFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 2) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        return _wrap_ModelCalibrationHelper_loadModelFromFile__SWIG_1(resc,resv,argc,argv);
+      }
+    }
+  }
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        int res = SWIG_AsPtr_std_string(argv[2], (std::string**)(0));
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelCalibrationHelper_loadModelFromFile__SWIG_0(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelCalibrationHelper_loadModelFromFile'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelCalibrationHelper::loadModelFromFile(std::string const &,std::string const &)\n"
+    "    iDynTree::ModelCalibrationHelper::loadModelFromFile(std::string const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToString__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  std::string arg4 ;
+  iDynTree::ModelExporterOptions arg5 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_updateModelInertialParametersToString",argc,5,5,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__string,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  arg2 = reinterpret_cast< std::string * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__ModelExporterOptions,  0 );
+    if (!SWIG_IsOK(res5)) {
+      SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "5"" of type '" "iDynTree::ModelExporterOptions const""'"); 
+    }  
+    if (!argp5) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "5"" of type '" "iDynTree::ModelExporterOptions const""'");
+    } else {
+      arg5 = *(reinterpret_cast< iDynTree::ModelExporterOptions * >(argp5));
+    }
+  }
+  result = (bool)(arg1)->updateModelInertialParametersToString(*arg2,(iDynTree::VectorDynSize const &)*arg3,arg4,arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToString__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  std::string arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_updateModelInertialParametersToString",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__string,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  arg2 = reinterpret_cast< std::string * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->updateModelInertialParametersToString(*arg2,(iDynTree::VectorDynSize const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToString__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  void *argp2 = 0 ;
+  int res2 = 0 ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_updateModelInertialParametersToString",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  res2 = SWIG_ConvertPtr(argv[1], &argp2, SWIGTYPE_p_std__string,  0 );
+  if (!SWIG_IsOK(res2)) {
+    SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  if (!argp2) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "2"" of type '" "std::string &""'"); 
+  }
+  arg2 = reinterpret_cast< std::string * >(argp2);
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToString" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  result = (bool)(arg1)->updateModelInertialParametersToString(*arg2,(iDynTree::VectorDynSize const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToString(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_std__string, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelCalibrationHelper_updateModelInertialParametersToString__SWIG_2(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_std__string, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ModelCalibrationHelper_updateModelInertialParametersToString__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      void *vptr = 0;
+      int res = SWIG_ConvertPtr(argv[1], &vptr, SWIGTYPE_p_std__string, 0);
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__ModelExporterOptions, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_ModelCalibrationHelper_updateModelInertialParametersToString__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelCalibrationHelper_updateModelInertialParametersToString'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelCalibrationHelper::updateModelInertialParametersToString(std::string &,iDynTree::VectorDynSize const &,std::string const,iDynTree::ModelExporterOptions const)\n"
+    "    iDynTree::ModelCalibrationHelper::updateModelInertialParametersToString(std::string &,iDynTree::VectorDynSize const &,std::string const)\n"
+    "    iDynTree::ModelCalibrationHelper::updateModelInertialParametersToString(std::string &,iDynTree::VectorDynSize const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  std::string arg4 ;
+  iDynTree::ModelExporterOptions arg5 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
+  void *argp5 ;
+  int res5 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_updateModelInertialParametersToFile",argc,5,5,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  {
+    res5 = SWIG_ConvertPtr(argv[4], &argp5, SWIGTYPE_p_iDynTree__ModelExporterOptions,  0 );
+    if (!SWIG_IsOK(res5)) {
+      SWIG_exception_fail(SWIG_ArgError(res5), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "5"" of type '" "iDynTree::ModelExporterOptions const""'"); 
+    }  
+    if (!argp5) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "5"" of type '" "iDynTree::ModelExporterOptions const""'");
+    } else {
+      arg5 = *(reinterpret_cast< iDynTree::ModelExporterOptions * >(argp5));
+    }
+  }
+  result = (bool)(arg1)->updateModelInertialParametersToFile((std::string const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,arg4,arg5);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile__SWIG_1(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  std::string arg4 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_updateModelInertialParametersToFile",argc,4,4,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  {
+    std::string *ptr = (std::string *)0;
+    int res = SWIG_AsPtr_std_string(argv[3], &ptr);
+    if (!SWIG_IsOK(res) || !ptr) {
+      SWIG_exception_fail(SWIG_ArgError((ptr ? res : SWIG_TypeError)), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "4"" of type '" "std::string const""'"); 
+    }
+    arg4 = *ptr;
+    if (SWIG_IsNewObj(res)) delete ptr;
+  }
+  result = (bool)(arg1)->updateModelInertialParametersToFile((std::string const &)*arg2,(iDynTree::VectorDynSize const &)*arg3,arg4);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile__SWIG_2(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  std::string *arg2 = 0 ;
+  iDynTree::VectorDynSize *arg3 = 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  int res2 = SWIG_OLDOBJ ;
+  void *argp3 ;
+  int res3 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_updateModelInertialParametersToFile",argc,3,3,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  {
+    std::string *ptr = (std::string *)0;
+    res2 = SWIG_AsPtr_std_string(argv[1], &ptr);
+    if (!SWIG_IsOK(res2)) {
+      SWIG_exception_fail(SWIG_ArgError(res2), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    if (!ptr) {
+      SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "2"" of type '" "std::string const &""'"); 
+    }
+    arg2 = ptr;
+  }
+  res3 = SWIG_ConvertPtr(argv[2], &argp3, SWIGTYPE_p_iDynTree__VectorDynSize,  0 );
+  if (!SWIG_IsOK(res3)) {
+    SWIG_exception_fail(SWIG_ArgError(res3), "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  if (!argp3) {
+    SWIG_exception_fail(SWIG_ValueError, "invalid null reference " "in method '" "ModelCalibrationHelper_updateModelInertialParametersToFile" "', argument " "3"" of type '" "iDynTree::VectorDynSize const &""'"); 
+  }
+  arg3 = reinterpret_cast< iDynTree::VectorDynSize * >(argp3);
+  result = (bool)(arg1)->updateModelInertialParametersToFile((std::string const &)*arg2,(iDynTree::VectorDynSize const &)*arg3);
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 0;
+fail:
+  if (SWIG_IsNewObj(res2)) delete arg2;
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  if (argc == 3) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          return _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile__SWIG_2(resc,resv,argc,argv);
+        }
+      }
+    }
+  }
+  if (argc == 4) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            return _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile__SWIG_1(resc,resv,argc,argv);
+          }
+        }
+      }
+    }
+  }
+  if (argc == 5) {
+    int _v;
+    void *vptr = 0;
+    int res = SWIG_ConvertPtr(argv[0], &vptr, SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0);
+    _v = SWIG_CheckState(res);
+    if (_v) {
+      int res = SWIG_AsPtr_std_string(argv[1], (std::string**)(0));
+      _v = SWIG_CheckState(res);
+      if (_v) {
+        void *vptr = 0;
+        int res = SWIG_ConvertPtr(argv[2], &vptr, SWIGTYPE_p_iDynTree__VectorDynSize, 0);
+        _v = SWIG_CheckState(res);
+        if (_v) {
+          int res = SWIG_AsPtr_std_string(argv[3], (std::string**)(0));
+          _v = SWIG_CheckState(res);
+          if (_v) {
+            void *vptr = 0;
+            int res = SWIG_ConvertPtr(argv[4], &vptr, SWIGTYPE_p_iDynTree__ModelExporterOptions, 0);
+            _v = SWIG_CheckState(res);
+            if (_v) {
+              return _wrap_ModelCalibrationHelper_updateModelInertialParametersToFile__SWIG_0(resc,resv,argc,argv);
+            }
+          }
+        }
+      }
+    }
+  }
+  
+  SWIG_Error(SWIG_RuntimeError, "No matching function for overload function 'ModelCalibrationHelper_updateModelInertialParametersToFile'."
+    "  Possible C/C++ prototypes are:\n"
+    "    iDynTree::ModelCalibrationHelper::updateModelInertialParametersToFile(std::string const &,iDynTree::VectorDynSize const &,std::string const,iDynTree::ModelExporterOptions const)\n"
+    "    iDynTree::ModelCalibrationHelper::updateModelInertialParametersToFile(std::string const &,iDynTree::VectorDynSize const &,std::string const)\n"
+    "    iDynTree::ModelCalibrationHelper::updateModelInertialParametersToFile(std::string const &,iDynTree::VectorDynSize const &)\n");
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_model(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::Model *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_model",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_model" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  result = (iDynTree::Model *) &(arg1)->model();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__Model, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_sensors(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  iDynTree::SensorsList *result = 0 ;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_sensors",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_sensors" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  result = (iDynTree::SensorsList *) &(arg1)->sensors();
+  _out = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_iDynTree__SensorsList, 0 |  0 );
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
+int _wrap_ModelCalibrationHelper_isValid(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
+  iDynTree::ModelCalibrationHelper *arg1 = (iDynTree::ModelCalibrationHelper *) 0 ;
+  void *argp1 = 0 ;
+  int res1 = 0 ;
+  mxArray * _out;
+  bool result;
+  
+  if (!SWIG_check_num_args("ModelCalibrationHelper_isValid",argc,1,1,0)) {
+    SWIG_fail;
+  }
+  res1 = SWIG_ConvertPtr(argv[0], &argp1,SWIGTYPE_p_iDynTree__ModelCalibrationHelper, 0 |  0 );
+  if (!SWIG_IsOK(res1)) {
+    SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "ModelCalibrationHelper_isValid" "', argument " "1"" of type '" "iDynTree::ModelCalibrationHelper *""'"); 
+  }
+  arg1 = reinterpret_cast< iDynTree::ModelCalibrationHelper * >(argp1);
+  result = (bool)(arg1)->isValid();
+  _out = SWIG_From_bool(static_cast< bool >(result));
+  if (_out) --resc, *resv++ = _out;
+  return 0;
+fail:
+  return 1;
+}
+
+
 int _wrap_new_UnknownWrenchContact__SWIG_0(int resc, mxArray *resv[], int argc, mxArray *argv[]) {
   mxArray * _out;
   iDynTree::UnknownWrenchContact *result = 0 ;
@@ -95833,6 +96733,7 @@ static swig_type_info _swigt__p_iDynTree__MatrixFixSizeT_6_10_t = {"_p_iDynTree_
 static swig_type_info _swigt__p_iDynTree__MatrixFixSizeT_6_1_t = {"_p_iDynTree__MatrixFixSizeT_6_1_t", "iDynTree::MatrixFixSize< 6,1 > *|iDynTree::Matrix6x1 *", 0, 0, (void*)0, 0};
 static swig_type_info _swigt__p_iDynTree__MatrixFixSizeT_6_6_t = {"_p_iDynTree__MatrixFixSizeT_6_6_t", "iDynTree::MatrixFixSize< 6,6 > *|iDynTree::Matrix6x6 *", 0, 0, (void*)"iDynTree.Matrix6x6", 0};
 static swig_type_info _swigt__p_iDynTree__Model = {"_p_iDynTree__Model", "iDynTree::Model *", 0, 0, (void*)"iDynTree.Model", 0};
+static swig_type_info _swigt__p_iDynTree__ModelCalibrationHelper = {"_p_iDynTree__ModelCalibrationHelper", "iDynTree::ModelCalibrationHelper *", 0, 0, (void*)"iDynTree.ModelCalibrationHelper", 0};
 static swig_type_info _swigt__p_iDynTree__ModelExporter = {"_p_iDynTree__ModelExporter", "iDynTree::ModelExporter *", 0, 0, (void*)"iDynTree.ModelExporter", 0};
 static swig_type_info _swigt__p_iDynTree__ModelExporterOptions = {"_p_iDynTree__ModelExporterOptions", "iDynTree::ModelExporterOptions *", 0, 0, (void*)"iDynTree.ModelExporterOptions", 0};
 static swig_type_info _swigt__p_iDynTree__ModelLoader = {"_p_iDynTree__ModelLoader", "iDynTree::ModelLoader *", 0, 0, (void*)"iDynTree.ModelLoader", 0};
@@ -96051,6 +96952,7 @@ static swig_type_info *swig_type_initial[] = {
   &_swigt__p_iDynTree__MatrixFixSizeT_6_1_t,
   &_swigt__p_iDynTree__MatrixFixSizeT_6_6_t,
   &_swigt__p_iDynTree__Model,
+  &_swigt__p_iDynTree__ModelCalibrationHelper,
   &_swigt__p_iDynTree__ModelExporter,
   &_swigt__p_iDynTree__ModelExporterOptions,
   &_swigt__p_iDynTree__ModelLoader,
@@ -96269,6 +97171,7 @@ static swig_cast_info _swigc__p_iDynTree__MatrixFixSizeT_6_10_t[] = {  {&_swigt_
 static swig_cast_info _swigc__p_iDynTree__MatrixFixSizeT_6_1_t[] = {  {&_swigt__p_iDynTree__MatrixFixSizeT_6_1_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__MatrixFixSizeT_6_6_t[] = {  {&_swigt__p_iDynTree__MatrixFixSizeT_6_6_t, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__Model[] = {  {&_swigt__p_iDynTree__Model, 0, 0, 0},{0, 0, 0, 0}};
+static swig_cast_info _swigc__p_iDynTree__ModelCalibrationHelper[] = {  {&_swigt__p_iDynTree__ModelCalibrationHelper, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ModelExporter[] = {  {&_swigt__p_iDynTree__ModelExporter, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ModelExporterOptions[] = {  {&_swigt__p_iDynTree__ModelExporterOptions, 0, 0, 0},{0, 0, 0, 0}};
 static swig_cast_info _swigc__p_iDynTree__ModelLoader[] = {  {&_swigt__p_iDynTree__ModelLoader, 0, 0, 0},{0, 0, 0, 0}};
@@ -96487,6 +97390,7 @@ static swig_cast_info *swig_cast_initial[] = {
   _swigc__p_iDynTree__MatrixFixSizeT_6_1_t,
   _swigc__p_iDynTree__MatrixFixSizeT_6_6_t,
   _swigc__p_iDynTree__Model,
+  _swigc__p_iDynTree__ModelCalibrationHelper,
   _swigc__p_iDynTree__ModelExporter,
   _swigc__p_iDynTree__ModelExporterOptions,
   _swigc__p_iDynTree__ModelLoader,
@@ -98478,523 +99382,532 @@ SWIGINTERN const char* SwigFunctionName(int fcn_id) {
   case 1487: return "ModelExporter_isValid";
   case 1488: return "ModelExporter_exportModelToString";
   case 1489: return "ModelExporter_exportModelToFile";
-  case 1490: return "new_UnknownWrenchContact";
-  case 1491: return "UnknownWrenchContact_unknownType_get";
-  case 1492: return "UnknownWrenchContact_unknownType_set";
-  case 1493: return "UnknownWrenchContact_contactPoint_get";
-  case 1494: return "UnknownWrenchContact_contactPoint_set";
-  case 1495: return "UnknownWrenchContact_forceDirection_get";
-  case 1496: return "UnknownWrenchContact_forceDirection_set";
-  case 1497: return "UnknownWrenchContact_knownWrench_get";
-  case 1498: return "UnknownWrenchContact_knownWrench_set";
-  case 1499: return "UnknownWrenchContact_contactId_get";
-  case 1500: return "UnknownWrenchContact_contactId_set";
-  case 1501: return "delete_UnknownWrenchContact";
-  case 1502: return "new_LinkUnknownWrenchContacts";
-  case 1503: return "LinkUnknownWrenchContacts_clear";
-  case 1504: return "LinkUnknownWrenchContacts_resize";
-  case 1505: return "LinkUnknownWrenchContacts_getNrOfContactsForLink";
-  case 1506: return "LinkUnknownWrenchContacts_setNrOfContactsForLink";
-  case 1507: return "LinkUnknownWrenchContacts_addNewContactForLink";
-  case 1508: return "LinkUnknownWrenchContacts_addNewContactInFrame";
-  case 1509: return "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin";
-  case 1510: return "LinkUnknownWrenchContacts_contactWrench";
-  case 1511: return "LinkUnknownWrenchContacts_toString";
-  case 1512: return "delete_LinkUnknownWrenchContacts";
-  case 1513: return "new_estimateExternalWrenchesBuffers";
-  case 1514: return "estimateExternalWrenchesBuffers_resize";
-  case 1515: return "estimateExternalWrenchesBuffers_getNrOfSubModels";
-  case 1516: return "estimateExternalWrenchesBuffers_getNrOfLinks";
-  case 1517: return "estimateExternalWrenchesBuffers_isConsistent";
-  case 1518: return "estimateExternalWrenchesBuffers_A_get";
-  case 1519: return "estimateExternalWrenchesBuffers_A_set";
-  case 1520: return "estimateExternalWrenchesBuffers_x_get";
-  case 1521: return "estimateExternalWrenchesBuffers_x_set";
-  case 1522: return "estimateExternalWrenchesBuffers_b_get";
-  case 1523: return "estimateExternalWrenchesBuffers_b_set";
-  case 1524: return "estimateExternalWrenchesBuffers_pinvA_get";
-  case 1525: return "estimateExternalWrenchesBuffers_pinvA_set";
-  case 1526: return "estimateExternalWrenchesBuffers_b_contacts_subtree_get";
-  case 1527: return "estimateExternalWrenchesBuffers_b_contacts_subtree_set";
-  case 1528: return "estimateExternalWrenchesBuffers_subModelBase_H_link_get";
-  case 1529: return "estimateExternalWrenchesBuffers_subModelBase_H_link_set";
-  case 1530: return "delete_estimateExternalWrenchesBuffers";
-  case 1531: return "_wrap_estimateExternalWrenchesWithoutInternalFT";
-  case 1532: return "_wrap_estimateExternalWrenches";
-  case 1533: return "_wrap_dynamicsEstimationForwardVelAccKinematics";
-  case 1534: return "_wrap_dynamicsEstimationForwardVelKinematics";
-  case 1535: return "_wrap_computeLinkNetWrenchesWithoutGravity";
-  case 1536: return "_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches";
-  case 1537: return "new_ExtWrenchesAndJointTorquesEstimator";
-  case 1538: return "delete_ExtWrenchesAndJointTorquesEstimator";
-  case 1539: return "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors";
-  case 1540: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile";
-  case 1541: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs";
-  case 1542: return "ExtWrenchesAndJointTorquesEstimator_model";
-  case 1543: return "ExtWrenchesAndJointTorquesEstimator_sensors";
-  case 1544: return "ExtWrenchesAndJointTorquesEstimator_submodels";
-  case 1545: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase";
-  case 1546: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase";
-  case 1547: return "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements";
-  case 1548: return "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques";
-  case 1549: return "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill";
-  case 1550: return "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity";
-  case 1551: return "new_SimpleLeggedOdometry";
-  case 1552: return "delete_SimpleLeggedOdometry";
-  case 1553: return "SimpleLeggedOdometry_setModel";
-  case 1554: return "SimpleLeggedOdometry_loadModelFromFile";
-  case 1555: return "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs";
-  case 1556: return "SimpleLeggedOdometry_model";
-  case 1557: return "SimpleLeggedOdometry_updateKinematics";
-  case 1558: return "SimpleLeggedOdometry_init";
-  case 1559: return "SimpleLeggedOdometry_changeFixedFrame";
-  case 1560: return "SimpleLeggedOdometry_getCurrentFixedLink";
-  case 1561: return "SimpleLeggedOdometry_getWorldLinkTransform";
-  case 1562: return "SimpleLeggedOdometry_getWorldFrameTransform";
-  case 1563: return "_wrap_isLinkBerdyDynamicVariable";
-  case 1564: return "_wrap_isJointBerdyDynamicVariable";
-  case 1565: return "_wrap_isDOFBerdyDynamicVariable";
-  case 1566: return "new_BerdyOptions";
-  case 1567: return "BerdyOptions_berdyVariant_get";
-  case 1568: return "BerdyOptions_berdyVariant_set";
-  case 1569: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get";
-  case 1570: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set";
-  case 1571: return "BerdyOptions_includeAllJointAccelerationsAsSensors_get";
-  case 1572: return "BerdyOptions_includeAllJointAccelerationsAsSensors_set";
-  case 1573: return "BerdyOptions_includeAllJointTorquesAsSensors_get";
-  case 1574: return "BerdyOptions_includeAllJointTorquesAsSensors_set";
-  case 1575: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get";
-  case 1576: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set";
-  case 1577: return "BerdyOptions_includeFixedBaseExternalWrench_get";
-  case 1578: return "BerdyOptions_includeFixedBaseExternalWrench_set";
-  case 1579: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get";
-  case 1580: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set";
-  case 1581: return "BerdyOptions_baseLink_get";
-  case 1582: return "BerdyOptions_baseLink_set";
-  case 1583: return "BerdyOptions_checkConsistency";
-  case 1584: return "delete_BerdyOptions";
-  case 1585: return "BerdySensor_type_get";
-  case 1586: return "BerdySensor_type_set";
-  case 1587: return "BerdySensor_id_get";
-  case 1588: return "BerdySensor_id_set";
-  case 1589: return "BerdySensor_range_get";
-  case 1590: return "BerdySensor_range_set";
-  case 1591: return "BerdySensor_eq";
-  case 1592: return "BerdySensor_lt";
-  case 1593: return "new_BerdySensor";
-  case 1594: return "delete_BerdySensor";
-  case 1595: return "BerdyDynamicVariable_type_get";
-  case 1596: return "BerdyDynamicVariable_type_set";
-  case 1597: return "BerdyDynamicVariable_id_get";
-  case 1598: return "BerdyDynamicVariable_id_set";
-  case 1599: return "BerdyDynamicVariable_range_get";
-  case 1600: return "BerdyDynamicVariable_range_set";
-  case 1601: return "BerdyDynamicVariable_eq";
-  case 1602: return "BerdyDynamicVariable_lt";
-  case 1603: return "new_BerdyDynamicVariable";
-  case 1604: return "delete_BerdyDynamicVariable";
-  case 1605: return "new_BerdyHelper";
-  case 1606: return "BerdyHelper_dynamicTraversal";
-  case 1607: return "BerdyHelper_model";
-  case 1608: return "BerdyHelper_sensors";
-  case 1609: return "BerdyHelper_isValid";
-  case 1610: return "BerdyHelper_init";
-  case 1611: return "BerdyHelper_getOptions";
-  case 1612: return "BerdyHelper_getNrOfDynamicVariables";
-  case 1613: return "BerdyHelper_getNrOfDynamicEquations";
-  case 1614: return "BerdyHelper_getNrOfSensorsMeasurements";
-  case 1615: return "BerdyHelper_resizeAndZeroBerdyMatrices";
-  case 1616: return "BerdyHelper_getBerdyMatrices";
-  case 1617: return "BerdyHelper_getSensorsOrdering";
-  case 1618: return "BerdyHelper_getRangeSensorVariable";
-  case 1619: return "BerdyHelper_getRangeDOFSensorVariable";
-  case 1620: return "BerdyHelper_getRangeJointSensorVariable";
-  case 1621: return "BerdyHelper_getRangeLinkSensorVariable";
-  case 1622: return "BerdyHelper_getRangeLinkVariable";
-  case 1623: return "BerdyHelper_getRangeJointVariable";
-  case 1624: return "BerdyHelper_getRangeDOFVariable";
-  case 1625: return "BerdyHelper_getDynamicVariablesOrdering";
-  case 1626: return "BerdyHelper_serializeDynamicVariables";
-  case 1627: return "BerdyHelper_serializeSensorVariables";
-  case 1628: return "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA";
-  case 1629: return "BerdyHelper_extractJointTorquesFromDynamicVariables";
-  case 1630: return "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables";
-  case 1631: return "BerdyHelper_updateKinematicsFromFloatingBase";
-  case 1632: return "BerdyHelper_updateKinematicsFromFixedBase";
-  case 1633: return "BerdyHelper_updateKinematicsFromTraversalFixedBase";
-  case 1634: return "BerdyHelper_setNetExternalWrenchMeasurementFrame";
-  case 1635: return "BerdyHelper_getNetExternalWrenchMeasurementFrame";
-  case 1636: return "delete_BerdyHelper";
-  case 1637: return "new_BerdySparseMAPSolver";
-  case 1638: return "delete_BerdySparseMAPSolver";
-  case 1639: return "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance";
-  case 1640: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance";
-  case 1641: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue";
-  case 1642: return "BerdySparseMAPSolver_setMeasurementsPriorCovariance";
-  case 1643: return "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse";
-  case 1644: return "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse";
-  case 1645: return "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue";
-  case 1646: return "BerdySparseMAPSolver_measurementsPriorCovarianceInverse";
-  case 1647: return "BerdySparseMAPSolver_isValid";
-  case 1648: return "BerdySparseMAPSolver_initialize";
-  case 1649: return "BerdySparseMAPSolver_updateEstimateInformationFixedBase";
-  case 1650: return "BerdySparseMAPSolver_updateEstimateInformationFloatingBase";
-  case 1651: return "BerdySparseMAPSolver_doEstimate";
-  case 1652: return "BerdySparseMAPSolver_getLastEstimate";
-  case 1653: return "AttitudeEstimatorState_m_orientation_get";
-  case 1654: return "AttitudeEstimatorState_m_orientation_set";
-  case 1655: return "AttitudeEstimatorState_m_angular_velocity_get";
-  case 1656: return "AttitudeEstimatorState_m_angular_velocity_set";
-  case 1657: return "AttitudeEstimatorState_m_gyroscope_bias_get";
-  case 1658: return "AttitudeEstimatorState_m_gyroscope_bias_set";
-  case 1659: return "new_AttitudeEstimatorState";
-  case 1660: return "delete_AttitudeEstimatorState";
-  case 1661: return "delete_IAttitudeEstimator";
-  case 1662: return "IAttitudeEstimator_updateFilterWithMeasurements";
-  case 1663: return "IAttitudeEstimator_propagateStates";
-  case 1664: return "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix";
-  case 1665: return "IAttitudeEstimator_getOrientationEstimateAsQuaternion";
-  case 1666: return "IAttitudeEstimator_getOrientationEstimateAsRPY";
-  case 1667: return "IAttitudeEstimator_getInternalStateSize";
-  case 1668: return "IAttitudeEstimator_getInternalState";
-  case 1669: return "IAttitudeEstimator_getDefaultInternalInitialState";
-  case 1670: return "IAttitudeEstimator_setInternalState";
-  case 1671: return "IAttitudeEstimator_setInternalStateInitialOrientation";
-  case 1672: return "AttitudeMahonyFilterParameters_time_step_in_seconds_get";
-  case 1673: return "AttitudeMahonyFilterParameters_time_step_in_seconds_set";
-  case 1674: return "AttitudeMahonyFilterParameters_kp_get";
-  case 1675: return "AttitudeMahonyFilterParameters_kp_set";
-  case 1676: return "AttitudeMahonyFilterParameters_ki_get";
-  case 1677: return "AttitudeMahonyFilterParameters_ki_set";
-  case 1678: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_get";
-  case 1679: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set";
-  case 1680: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get";
-  case 1681: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set";
-  case 1682: return "new_AttitudeMahonyFilterParameters";
-  case 1683: return "delete_AttitudeMahonyFilterParameters";
-  case 1684: return "new_AttitudeMahonyFilter";
-  case 1685: return "AttitudeMahonyFilter_useMagnetoMeterMeasurements";
-  case 1686: return "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements";
-  case 1687: return "AttitudeMahonyFilter_setGainkp";
-  case 1688: return "AttitudeMahonyFilter_setGainki";
-  case 1689: return "AttitudeMahonyFilter_setTimeStepInSeconds";
-  case 1690: return "AttitudeMahonyFilter_setGravityDirection";
-  case 1691: return "AttitudeMahonyFilter_setParameters";
-  case 1692: return "AttitudeMahonyFilter_getParameters";
-  case 1693: return "AttitudeMahonyFilter_updateFilterWithMeasurements";
-  case 1694: return "AttitudeMahonyFilter_propagateStates";
-  case 1695: return "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix";
-  case 1696: return "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion";
-  case 1697: return "AttitudeMahonyFilter_getOrientationEstimateAsRPY";
-  case 1698: return "AttitudeMahonyFilter_getInternalStateSize";
-  case 1699: return "AttitudeMahonyFilter_getInternalState";
-  case 1700: return "AttitudeMahonyFilter_getDefaultInternalInitialState";
-  case 1701: return "AttitudeMahonyFilter_setInternalState";
-  case 1702: return "AttitudeMahonyFilter_setInternalStateInitialOrientation";
-  case 1703: return "delete_AttitudeMahonyFilter";
-  case 1704: return "DiscreteExtendedKalmanFilterHelper_ekf_f";
-  case 1705: return "DiscreteExtendedKalmanFilterHelper_ekf_h";
-  case 1706: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF";
-  case 1707: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH";
-  case 1708: return "DiscreteExtendedKalmanFilterHelper_ekfPredict";
-  case 1709: return "DiscreteExtendedKalmanFilterHelper_ekfUpdate";
-  case 1710: return "DiscreteExtendedKalmanFilterHelper_ekfInit";
-  case 1711: return "DiscreteExtendedKalmanFilterHelper_ekfReset";
-  case 1712: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector";
-  case 1713: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector";
-  case 1714: return "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState";
-  case 1715: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance";
-  case 1716: return "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance";
-  case 1717: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance";
-  case 1718: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize";
-  case 1719: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize";
-  case 1720: return "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize";
-  case 1721: return "DiscreteExtendedKalmanFilterHelper_ekfGetStates";
-  case 1722: return "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance";
-  case 1723: return "delete_DiscreteExtendedKalmanFilterHelper";
-  case 1724: return "output_dimensions_with_magnetometer_get";
-  case 1725: return "output_dimensions_without_magnetometer_get";
-  case 1726: return "input_dimensions_get";
-  case 1727: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_get";
-  case 1728: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_set";
-  case 1729: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get";
-  case 1730: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set";
-  case 1731: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get";
-  case 1732: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set";
-  case 1733: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get";
-  case 1734: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set";
-  case 1735: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get";
-  case 1736: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set";
-  case 1737: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get";
-  case 1738: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set";
-  case 1739: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get";
-  case 1740: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set";
-  case 1741: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get";
-  case 1742: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set";
-  case 1743: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get";
-  case 1744: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set";
-  case 1745: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get";
-  case 1746: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set";
-  case 1747: return "new_AttitudeQuaternionEKFParameters";
-  case 1748: return "delete_AttitudeQuaternionEKFParameters";
-  case 1749: return "new_AttitudeQuaternionEKF";
-  case 1750: return "AttitudeQuaternionEKF_getParameters";
-  case 1751: return "AttitudeQuaternionEKF_setParameters";
-  case 1752: return "AttitudeQuaternionEKF_setGravityDirection";
-  case 1753: return "AttitudeQuaternionEKF_setTimeStepInSeconds";
-  case 1754: return "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor";
-  case 1755: return "AttitudeQuaternionEKF_useMagnetometerMeasurements";
-  case 1756: return "AttitudeQuaternionEKF_setMeasurementNoiseVariance";
-  case 1757: return "AttitudeQuaternionEKF_setSystemNoiseVariance";
-  case 1758: return "AttitudeQuaternionEKF_setInitialStateCovariance";
-  case 1759: return "AttitudeQuaternionEKF_initializeFilter";
-  case 1760: return "AttitudeQuaternionEKF_updateFilterWithMeasurements";
-  case 1761: return "AttitudeQuaternionEKF_propagateStates";
-  case 1762: return "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix";
-  case 1763: return "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion";
-  case 1764: return "AttitudeQuaternionEKF_getOrientationEstimateAsRPY";
-  case 1765: return "AttitudeQuaternionEKF_getInternalStateSize";
-  case 1766: return "AttitudeQuaternionEKF_getInternalState";
-  case 1767: return "AttitudeQuaternionEKF_getDefaultInternalInitialState";
-  case 1768: return "AttitudeQuaternionEKF_setInternalState";
-  case 1769: return "AttitudeQuaternionEKF_setInternalStateInitialOrientation";
-  case 1770: return "delete_AttitudeQuaternionEKF";
-  case 1771: return "_wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass";
-  case 1772: return "DynamicsRegressorParameter_category_get";
-  case 1773: return "DynamicsRegressorParameter_category_set";
-  case 1774: return "DynamicsRegressorParameter_elemIndex_get";
-  case 1775: return "DynamicsRegressorParameter_elemIndex_set";
-  case 1776: return "DynamicsRegressorParameter_type_get";
-  case 1777: return "DynamicsRegressorParameter_type_set";
-  case 1778: return "DynamicsRegressorParameter_lt";
-  case 1779: return "DynamicsRegressorParameter_eq";
-  case 1780: return "DynamicsRegressorParameter_ne";
-  case 1781: return "new_DynamicsRegressorParameter";
-  case 1782: return "delete_DynamicsRegressorParameter";
-  case 1783: return "DynamicsRegressorParametersList_parameters_get";
-  case 1784: return "DynamicsRegressorParametersList_parameters_set";
-  case 1785: return "DynamicsRegressorParametersList_getDescriptionOfParameter";
-  case 1786: return "DynamicsRegressorParametersList_addParam";
-  case 1787: return "DynamicsRegressorParametersList_addList";
-  case 1788: return "DynamicsRegressorParametersList_findParam";
-  case 1789: return "DynamicsRegressorParametersList_getNrOfParameters";
-  case 1790: return "new_DynamicsRegressorParametersList";
-  case 1791: return "delete_DynamicsRegressorParametersList";
-  case 1792: return "new_DynamicsRegressorGenerator";
-  case 1793: return "delete_DynamicsRegressorGenerator";
-  case 1794: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile";
-  case 1795: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString";
-  case 1796: return "DynamicsRegressorGenerator_loadRegressorStructureFromFile";
-  case 1797: return "DynamicsRegressorGenerator_loadRegressorStructureFromString";
-  case 1798: return "DynamicsRegressorGenerator_isValid";
-  case 1799: return "DynamicsRegressorGenerator_getNrOfParameters";
-  case 1800: return "DynamicsRegressorGenerator_getNrOfOutputs";
-  case 1801: return "DynamicsRegressorGenerator_getNrOfDegreesOfFreedom";
-  case 1802: return "DynamicsRegressorGenerator_getDescriptionOfParameter";
-  case 1803: return "DynamicsRegressorGenerator_getDescriptionOfParameters";
-  case 1804: return "DynamicsRegressorGenerator_getDescriptionOfOutput";
-  case 1805: return "DynamicsRegressorGenerator_getDescriptionOfOutputs";
-  case 1806: return "DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom";
-  case 1807: return "DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom";
-  case 1808: return "DynamicsRegressorGenerator_getDescriptionOfLink";
-  case 1809: return "DynamicsRegressorGenerator_getDescriptionOfLinks";
-  case 1810: return "DynamicsRegressorGenerator_getNrOfLinks";
-  case 1811: return "DynamicsRegressorGenerator_getNrOfFakeLinks";
-  case 1812: return "DynamicsRegressorGenerator_getBaseLinkName";
-  case 1813: return "DynamicsRegressorGenerator_getSensorsModel";
-  case 1814: return "DynamicsRegressorGenerator_setRobotState";
-  case 1815: return "DynamicsRegressorGenerator_getSensorsMeasurements";
-  case 1816: return "DynamicsRegressorGenerator_setTorqueSensorMeasurement";
-  case 1817: return "DynamicsRegressorGenerator_computeRegressor";
-  case 1818: return "DynamicsRegressorGenerator_getModelParameters";
-  case 1819: return "DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace";
-  case 1820: return "DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace";
-  case 1821: return "DynamicsRegressorGenerator_generate_random_regressors";
-  case 1822: return "new_KinDynComputations";
-  case 1823: return "delete_KinDynComputations";
-  case 1824: return "KinDynComputations_loadRobotModel";
-  case 1825: return "KinDynComputations_loadRobotModelFromFile";
-  case 1826: return "KinDynComputations_loadRobotModelFromString";
-  case 1827: return "KinDynComputations_isValid";
-  case 1828: return "KinDynComputations_setFrameVelocityRepresentation";
-  case 1829: return "KinDynComputations_getFrameVelocityRepresentation";
-  case 1830: return "KinDynComputations_getNrOfDegreesOfFreedom";
-  case 1831: return "KinDynComputations_getDescriptionOfDegreeOfFreedom";
-  case 1832: return "KinDynComputations_getDescriptionOfDegreesOfFreedom";
-  case 1833: return "KinDynComputations_getNrOfLinks";
-  case 1834: return "KinDynComputations_getNrOfFrames";
-  case 1835: return "KinDynComputations_getFloatingBase";
-  case 1836: return "KinDynComputations_setFloatingBase";
-  case 1837: return "KinDynComputations_model";
-  case 1838: return "KinDynComputations_getRobotModel";
-  case 1839: return "KinDynComputations_getRelativeJacobianSparsityPattern";
-  case 1840: return "KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern";
-  case 1841: return "KinDynComputations_setJointPos";
-  case 1842: return "KinDynComputations_setRobotState";
-  case 1843: return "KinDynComputations_getRobotState";
-  case 1844: return "KinDynComputations_getWorldBaseTransform";
-  case 1845: return "KinDynComputations_getBaseTwist";
-  case 1846: return "KinDynComputations_getJointPos";
-  case 1847: return "KinDynComputations_getJointVel";
-  case 1848: return "KinDynComputations_getModelVel";
-  case 1849: return "KinDynComputations_getFrameIndex";
-  case 1850: return "KinDynComputations_getFrameName";
-  case 1851: return "KinDynComputations_getWorldTransform";
-  case 1852: return "KinDynComputations_getRelativeTransformExplicit";
-  case 1853: return "KinDynComputations_getRelativeTransform";
-  case 1854: return "KinDynComputations_getFrameVel";
-  case 1855: return "KinDynComputations_getFrameAcc";
-  case 1856: return "KinDynComputations_getFrameFreeFloatingJacobian";
-  case 1857: return "KinDynComputations_getRelativeJacobian";
-  case 1858: return "KinDynComputations_getRelativeJacobianExplicit";
-  case 1859: return "KinDynComputations_getFrameBiasAcc";
-  case 1860: return "KinDynComputations_getCenterOfMassPosition";
-  case 1861: return "KinDynComputations_getCenterOfMassVelocity";
-  case 1862: return "KinDynComputations_getCenterOfMassJacobian";
-  case 1863: return "KinDynComputations_getCenterOfMassBiasAcc";
-  case 1864: return "KinDynComputations_getAverageVelocity";
-  case 1865: return "KinDynComputations_getAverageVelocityJacobian";
-  case 1866: return "KinDynComputations_getCentroidalAverageVelocity";
-  case 1867: return "KinDynComputations_getCentroidalAverageVelocityJacobian";
-  case 1868: return "KinDynComputations_getLinearAngularMomentum";
-  case 1869: return "KinDynComputations_getLinearAngularMomentumJacobian";
-  case 1870: return "KinDynComputations_getCentroidalTotalMomentum";
-  case 1871: return "KinDynComputations_getFreeFloatingMassMatrix";
-  case 1872: return "KinDynComputations_inverseDynamics";
-  case 1873: return "KinDynComputations_generalizedBiasForces";
-  case 1874: return "KinDynComputations_generalizedGravityForces";
-  case 1875: return "KinDynComputations_generalizedExternalForces";
-  case 1876: return "KinDynComputations_inverseDynamicsInertialParametersRegressor";
-  case 1877: return "delete_ICamera";
-  case 1878: return "ICamera_setPosition";
-  case 1879: return "ICamera_setTarget";
-  case 1880: return "ICamera_setUpVector";
-  case 1881: return "ColorViz_r_get";
-  case 1882: return "ColorViz_r_set";
-  case 1883: return "ColorViz_g_get";
-  case 1884: return "ColorViz_g_set";
-  case 1885: return "ColorViz_b_get";
-  case 1886: return "ColorViz_b_set";
-  case 1887: return "ColorViz_a_get";
-  case 1888: return "ColorViz_a_set";
-  case 1889: return "new_ColorViz";
-  case 1890: return "delete_ColorViz";
-  case 1891: return "delete_ILight";
-  case 1892: return "ILight_getName";
-  case 1893: return "ILight_setType";
-  case 1894: return "ILight_getType";
-  case 1895: return "ILight_setPosition";
-  case 1896: return "ILight_getPosition";
-  case 1897: return "ILight_setDirection";
-  case 1898: return "ILight_getDirection";
-  case 1899: return "ILight_setAmbientColor";
-  case 1900: return "ILight_getAmbientColor";
-  case 1901: return "ILight_setSpecularColor";
-  case 1902: return "ILight_getSpecularColor";
-  case 1903: return "ILight_setDiffuseColor";
-  case 1904: return "ILight_getDiffuseColor";
-  case 1905: return "delete_IEnvironment";
-  case 1906: return "IEnvironment_getElements";
-  case 1907: return "IEnvironment_setElementVisibility";
-  case 1908: return "IEnvironment_setBackgroundColor";
-  case 1909: return "IEnvironment_setAmbientLight";
-  case 1910: return "IEnvironment_getLights";
-  case 1911: return "IEnvironment_addLight";
-  case 1912: return "IEnvironment_lightViz";
-  case 1913: return "IEnvironment_removeLight";
-  case 1914: return "delete_IJetsVisualization";
-  case 1915: return "IJetsVisualization_setJetsFrames";
-  case 1916: return "IJetsVisualization_getNrOfJets";
-  case 1917: return "IJetsVisualization_getJetDirection";
-  case 1918: return "IJetsVisualization_setJetDirection";
-  case 1919: return "IJetsVisualization_setJetColor";
-  case 1920: return "IJetsVisualization_setJetsDimensions";
-  case 1921: return "IJetsVisualization_setJetsIntensity";
-  case 1922: return "delete_IVectorsVisualization";
-  case 1923: return "IVectorsVisualization_addVector";
-  case 1924: return "IVectorsVisualization_getNrOfVectors";
-  case 1925: return "IVectorsVisualization_getVector";
-  case 1926: return "IVectorsVisualization_updateVector";
-  case 1927: return "IVectorsVisualization_setVectorColor";
-  case 1928: return "IVectorsVisualization_setVectorsAspect";
-  case 1929: return "delete_IModelVisualization";
-  case 1930: return "IModelVisualization_setPositions";
-  case 1931: return "IModelVisualization_setLinkPositions";
-  case 1932: return "IModelVisualization_model";
-  case 1933: return "IModelVisualization_getInstanceName";
-  case 1934: return "IModelVisualization_setModelVisibility";
-  case 1935: return "IModelVisualization_setModelColor";
-  case 1936: return "IModelVisualization_resetModelColor";
-  case 1937: return "IModelVisualization_setLinkColor";
-  case 1938: return "IModelVisualization_resetLinkColor";
-  case 1939: return "IModelVisualization_getLinkNames";
-  case 1940: return "IModelVisualization_setLinkVisibility";
-  case 1941: return "IModelVisualization_getFeatures";
-  case 1942: return "IModelVisualization_setFeatureVisibility";
-  case 1943: return "IModelVisualization_jets";
-  case 1944: return "IModelVisualization_getWorldModelTransform";
-  case 1945: return "IModelVisualization_getWorldLinkTransform";
-  case 1946: return "VisualizerOptions_verbose_get";
-  case 1947: return "VisualizerOptions_verbose_set";
-  case 1948: return "VisualizerOptions_winWidth_get";
-  case 1949: return "VisualizerOptions_winWidth_set";
-  case 1950: return "VisualizerOptions_winHeight_get";
-  case 1951: return "VisualizerOptions_winHeight_set";
-  case 1952: return "VisualizerOptions_rootFrameArrowsDimension_get";
-  case 1953: return "VisualizerOptions_rootFrameArrowsDimension_set";
-  case 1954: return "new_VisualizerOptions";
-  case 1955: return "delete_VisualizerOptions";
-  case 1956: return "new_Visualizer";
-  case 1957: return "delete_Visualizer";
-  case 1958: return "Visualizer_init";
-  case 1959: return "Visualizer_getNrOfVisualizedModels";
-  case 1960: return "Visualizer_getModelInstanceName";
-  case 1961: return "Visualizer_getModelInstanceIndex";
-  case 1962: return "Visualizer_addModel";
-  case 1963: return "Visualizer_modelViz";
-  case 1964: return "Visualizer_camera";
-  case 1965: return "Visualizer_enviroment";
-  case 1966: return "Visualizer_vectors";
-  case 1967: return "Visualizer_run";
-  case 1968: return "Visualizer_draw";
-  case 1969: return "Visualizer_drawToFile";
-  case 1970: return "Visualizer_close";
-  case 1971: return "new_DynamicsComputations";
-  case 1972: return "delete_DynamicsComputations";
-  case 1973: return "DynamicsComputations_loadRobotModelFromFile";
-  case 1974: return "DynamicsComputations_loadRobotModelFromString";
-  case 1975: return "DynamicsComputations_isValid";
-  case 1976: return "DynamicsComputations_getNrOfDegreesOfFreedom";
-  case 1977: return "DynamicsComputations_getDescriptionOfDegreeOfFreedom";
-  case 1978: return "DynamicsComputations_getDescriptionOfDegreesOfFreedom";
-  case 1979: return "DynamicsComputations_getNrOfLinks";
-  case 1980: return "DynamicsComputations_getNrOfFrames";
-  case 1981: return "DynamicsComputations_getFloatingBase";
-  case 1982: return "DynamicsComputations_setFloatingBase";
-  case 1983: return "DynamicsComputations_setRobotState";
-  case 1984: return "DynamicsComputations_getWorldBaseTransform";
-  case 1985: return "DynamicsComputations_getBaseTwist";
-  case 1986: return "DynamicsComputations_getJointPos";
-  case 1987: return "DynamicsComputations_getJointVel";
-  case 1988: return "DynamicsComputations_getFrameIndex";
-  case 1989: return "DynamicsComputations_getFrameName";
-  case 1990: return "DynamicsComputations_getWorldTransform";
-  case 1991: return "DynamicsComputations_getRelativeTransform";
-  case 1992: return "DynamicsComputations_getFrameTwist";
-  case 1993: return "DynamicsComputations_getFrameTwistInWorldOrient";
-  case 1994: return "DynamicsComputations_getFrameProperSpatialAcceleration";
-  case 1995: return "DynamicsComputations_getLinkIndex";
-  case 1996: return "DynamicsComputations_getLinkInertia";
-  case 1997: return "DynamicsComputations_getJointIndex";
-  case 1998: return "DynamicsComputations_getJointName";
-  case 1999: return "DynamicsComputations_getJointLimits";
-  case 2000: return "DynamicsComputations_inverseDynamics";
-  case 2001: return "DynamicsComputations_getFreeFloatingMassMatrix";
-  case 2002: return "DynamicsComputations_getFrameJacobian";
-  case 2003: return "DynamicsComputations_getDynamicsRegressor";
-  case 2004: return "DynamicsComputations_getModelDynamicsParameters";
-  case 2005: return "DynamicsComputations_getCenterOfMass";
-  case 2006: return "DynamicsComputations_getCenterOfMassJacobian";
+  case 1490: return "new_ModelCalibrationHelper";
+  case 1491: return "delete_ModelCalibrationHelper";
+  case 1492: return "ModelCalibrationHelper_loadModelFromString";
+  case 1493: return "ModelCalibrationHelper_loadModelFromFile";
+  case 1494: return "ModelCalibrationHelper_updateModelInertialParametersToString";
+  case 1495: return "ModelCalibrationHelper_updateModelInertialParametersToFile";
+  case 1496: return "ModelCalibrationHelper_model";
+  case 1497: return "ModelCalibrationHelper_sensors";
+  case 1498: return "ModelCalibrationHelper_isValid";
+  case 1499: return "new_UnknownWrenchContact";
+  case 1500: return "UnknownWrenchContact_unknownType_get";
+  case 1501: return "UnknownWrenchContact_unknownType_set";
+  case 1502: return "UnknownWrenchContact_contactPoint_get";
+  case 1503: return "UnknownWrenchContact_contactPoint_set";
+  case 1504: return "UnknownWrenchContact_forceDirection_get";
+  case 1505: return "UnknownWrenchContact_forceDirection_set";
+  case 1506: return "UnknownWrenchContact_knownWrench_get";
+  case 1507: return "UnknownWrenchContact_knownWrench_set";
+  case 1508: return "UnknownWrenchContact_contactId_get";
+  case 1509: return "UnknownWrenchContact_contactId_set";
+  case 1510: return "delete_UnknownWrenchContact";
+  case 1511: return "new_LinkUnknownWrenchContacts";
+  case 1512: return "LinkUnknownWrenchContacts_clear";
+  case 1513: return "LinkUnknownWrenchContacts_resize";
+  case 1514: return "LinkUnknownWrenchContacts_getNrOfContactsForLink";
+  case 1515: return "LinkUnknownWrenchContacts_setNrOfContactsForLink";
+  case 1516: return "LinkUnknownWrenchContacts_addNewContactForLink";
+  case 1517: return "LinkUnknownWrenchContacts_addNewContactInFrame";
+  case 1518: return "LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin";
+  case 1519: return "LinkUnknownWrenchContacts_contactWrench";
+  case 1520: return "LinkUnknownWrenchContacts_toString";
+  case 1521: return "delete_LinkUnknownWrenchContacts";
+  case 1522: return "new_estimateExternalWrenchesBuffers";
+  case 1523: return "estimateExternalWrenchesBuffers_resize";
+  case 1524: return "estimateExternalWrenchesBuffers_getNrOfSubModels";
+  case 1525: return "estimateExternalWrenchesBuffers_getNrOfLinks";
+  case 1526: return "estimateExternalWrenchesBuffers_isConsistent";
+  case 1527: return "estimateExternalWrenchesBuffers_A_get";
+  case 1528: return "estimateExternalWrenchesBuffers_A_set";
+  case 1529: return "estimateExternalWrenchesBuffers_x_get";
+  case 1530: return "estimateExternalWrenchesBuffers_x_set";
+  case 1531: return "estimateExternalWrenchesBuffers_b_get";
+  case 1532: return "estimateExternalWrenchesBuffers_b_set";
+  case 1533: return "estimateExternalWrenchesBuffers_pinvA_get";
+  case 1534: return "estimateExternalWrenchesBuffers_pinvA_set";
+  case 1535: return "estimateExternalWrenchesBuffers_b_contacts_subtree_get";
+  case 1536: return "estimateExternalWrenchesBuffers_b_contacts_subtree_set";
+  case 1537: return "estimateExternalWrenchesBuffers_subModelBase_H_link_get";
+  case 1538: return "estimateExternalWrenchesBuffers_subModelBase_H_link_set";
+  case 1539: return "delete_estimateExternalWrenchesBuffers";
+  case 1540: return "_wrap_estimateExternalWrenchesWithoutInternalFT";
+  case 1541: return "_wrap_estimateExternalWrenches";
+  case 1542: return "_wrap_dynamicsEstimationForwardVelAccKinematics";
+  case 1543: return "_wrap_dynamicsEstimationForwardVelKinematics";
+  case 1544: return "_wrap_computeLinkNetWrenchesWithoutGravity";
+  case 1545: return "_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches";
+  case 1546: return "new_ExtWrenchesAndJointTorquesEstimator";
+  case 1547: return "delete_ExtWrenchesAndJointTorquesEstimator";
+  case 1548: return "ExtWrenchesAndJointTorquesEstimator_setModelAndSensors";
+  case 1549: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile";
+  case 1550: return "ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs";
+  case 1551: return "ExtWrenchesAndJointTorquesEstimator_model";
+  case 1552: return "ExtWrenchesAndJointTorquesEstimator_sensors";
+  case 1553: return "ExtWrenchesAndJointTorquesEstimator_submodels";
+  case 1554: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase";
+  case 1555: return "ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase";
+  case 1556: return "ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements";
+  case 1557: return "ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques";
+  case 1558: return "ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill";
+  case 1559: return "ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity";
+  case 1560: return "new_SimpleLeggedOdometry";
+  case 1561: return "delete_SimpleLeggedOdometry";
+  case 1562: return "SimpleLeggedOdometry_setModel";
+  case 1563: return "SimpleLeggedOdometry_loadModelFromFile";
+  case 1564: return "SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs";
+  case 1565: return "SimpleLeggedOdometry_model";
+  case 1566: return "SimpleLeggedOdometry_updateKinematics";
+  case 1567: return "SimpleLeggedOdometry_init";
+  case 1568: return "SimpleLeggedOdometry_changeFixedFrame";
+  case 1569: return "SimpleLeggedOdometry_getCurrentFixedLink";
+  case 1570: return "SimpleLeggedOdometry_getWorldLinkTransform";
+  case 1571: return "SimpleLeggedOdometry_getWorldFrameTransform";
+  case 1572: return "_wrap_isLinkBerdyDynamicVariable";
+  case 1573: return "_wrap_isJointBerdyDynamicVariable";
+  case 1574: return "_wrap_isDOFBerdyDynamicVariable";
+  case 1575: return "new_BerdyOptions";
+  case 1576: return "BerdyOptions_berdyVariant_get";
+  case 1577: return "BerdyOptions_berdyVariant_set";
+  case 1578: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get";
+  case 1579: return "BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set";
+  case 1580: return "BerdyOptions_includeAllJointAccelerationsAsSensors_get";
+  case 1581: return "BerdyOptions_includeAllJointAccelerationsAsSensors_set";
+  case 1582: return "BerdyOptions_includeAllJointTorquesAsSensors_get";
+  case 1583: return "BerdyOptions_includeAllJointTorquesAsSensors_set";
+  case 1584: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_get";
+  case 1585: return "BerdyOptions_includeAllNetExternalWrenchesAsSensors_set";
+  case 1586: return "BerdyOptions_includeFixedBaseExternalWrench_get";
+  case 1587: return "BerdyOptions_includeFixedBaseExternalWrench_set";
+  case 1588: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get";
+  case 1589: return "BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set";
+  case 1590: return "BerdyOptions_baseLink_get";
+  case 1591: return "BerdyOptions_baseLink_set";
+  case 1592: return "BerdyOptions_checkConsistency";
+  case 1593: return "delete_BerdyOptions";
+  case 1594: return "BerdySensor_type_get";
+  case 1595: return "BerdySensor_type_set";
+  case 1596: return "BerdySensor_id_get";
+  case 1597: return "BerdySensor_id_set";
+  case 1598: return "BerdySensor_range_get";
+  case 1599: return "BerdySensor_range_set";
+  case 1600: return "BerdySensor_eq";
+  case 1601: return "BerdySensor_lt";
+  case 1602: return "new_BerdySensor";
+  case 1603: return "delete_BerdySensor";
+  case 1604: return "BerdyDynamicVariable_type_get";
+  case 1605: return "BerdyDynamicVariable_type_set";
+  case 1606: return "BerdyDynamicVariable_id_get";
+  case 1607: return "BerdyDynamicVariable_id_set";
+  case 1608: return "BerdyDynamicVariable_range_get";
+  case 1609: return "BerdyDynamicVariable_range_set";
+  case 1610: return "BerdyDynamicVariable_eq";
+  case 1611: return "BerdyDynamicVariable_lt";
+  case 1612: return "new_BerdyDynamicVariable";
+  case 1613: return "delete_BerdyDynamicVariable";
+  case 1614: return "new_BerdyHelper";
+  case 1615: return "BerdyHelper_dynamicTraversal";
+  case 1616: return "BerdyHelper_model";
+  case 1617: return "BerdyHelper_sensors";
+  case 1618: return "BerdyHelper_isValid";
+  case 1619: return "BerdyHelper_init";
+  case 1620: return "BerdyHelper_getOptions";
+  case 1621: return "BerdyHelper_getNrOfDynamicVariables";
+  case 1622: return "BerdyHelper_getNrOfDynamicEquations";
+  case 1623: return "BerdyHelper_getNrOfSensorsMeasurements";
+  case 1624: return "BerdyHelper_resizeAndZeroBerdyMatrices";
+  case 1625: return "BerdyHelper_getBerdyMatrices";
+  case 1626: return "BerdyHelper_getSensorsOrdering";
+  case 1627: return "BerdyHelper_getRangeSensorVariable";
+  case 1628: return "BerdyHelper_getRangeDOFSensorVariable";
+  case 1629: return "BerdyHelper_getRangeJointSensorVariable";
+  case 1630: return "BerdyHelper_getRangeLinkSensorVariable";
+  case 1631: return "BerdyHelper_getRangeLinkVariable";
+  case 1632: return "BerdyHelper_getRangeJointVariable";
+  case 1633: return "BerdyHelper_getRangeDOFVariable";
+  case 1634: return "BerdyHelper_getDynamicVariablesOrdering";
+  case 1635: return "BerdyHelper_serializeDynamicVariables";
+  case 1636: return "BerdyHelper_serializeSensorVariables";
+  case 1637: return "BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA";
+  case 1638: return "BerdyHelper_extractJointTorquesFromDynamicVariables";
+  case 1639: return "BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables";
+  case 1640: return "BerdyHelper_updateKinematicsFromFloatingBase";
+  case 1641: return "BerdyHelper_updateKinematicsFromFixedBase";
+  case 1642: return "BerdyHelper_updateKinematicsFromTraversalFixedBase";
+  case 1643: return "BerdyHelper_setNetExternalWrenchMeasurementFrame";
+  case 1644: return "BerdyHelper_getNetExternalWrenchMeasurementFrame";
+  case 1645: return "delete_BerdyHelper";
+  case 1646: return "new_BerdySparseMAPSolver";
+  case 1647: return "delete_BerdySparseMAPSolver";
+  case 1648: return "BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance";
+  case 1649: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance";
+  case 1650: return "BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue";
+  case 1651: return "BerdySparseMAPSolver_setMeasurementsPriorCovariance";
+  case 1652: return "BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse";
+  case 1653: return "BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse";
+  case 1654: return "BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue";
+  case 1655: return "BerdySparseMAPSolver_measurementsPriorCovarianceInverse";
+  case 1656: return "BerdySparseMAPSolver_isValid";
+  case 1657: return "BerdySparseMAPSolver_initialize";
+  case 1658: return "BerdySparseMAPSolver_updateEstimateInformationFixedBase";
+  case 1659: return "BerdySparseMAPSolver_updateEstimateInformationFloatingBase";
+  case 1660: return "BerdySparseMAPSolver_doEstimate";
+  case 1661: return "BerdySparseMAPSolver_getLastEstimate";
+  case 1662: return "AttitudeEstimatorState_m_orientation_get";
+  case 1663: return "AttitudeEstimatorState_m_orientation_set";
+  case 1664: return "AttitudeEstimatorState_m_angular_velocity_get";
+  case 1665: return "AttitudeEstimatorState_m_angular_velocity_set";
+  case 1666: return "AttitudeEstimatorState_m_gyroscope_bias_get";
+  case 1667: return "AttitudeEstimatorState_m_gyroscope_bias_set";
+  case 1668: return "new_AttitudeEstimatorState";
+  case 1669: return "delete_AttitudeEstimatorState";
+  case 1670: return "delete_IAttitudeEstimator";
+  case 1671: return "IAttitudeEstimator_updateFilterWithMeasurements";
+  case 1672: return "IAttitudeEstimator_propagateStates";
+  case 1673: return "IAttitudeEstimator_getOrientationEstimateAsRotationMatrix";
+  case 1674: return "IAttitudeEstimator_getOrientationEstimateAsQuaternion";
+  case 1675: return "IAttitudeEstimator_getOrientationEstimateAsRPY";
+  case 1676: return "IAttitudeEstimator_getInternalStateSize";
+  case 1677: return "IAttitudeEstimator_getInternalState";
+  case 1678: return "IAttitudeEstimator_getDefaultInternalInitialState";
+  case 1679: return "IAttitudeEstimator_setInternalState";
+  case 1680: return "IAttitudeEstimator_setInternalStateInitialOrientation";
+  case 1681: return "AttitudeMahonyFilterParameters_time_step_in_seconds_get";
+  case 1682: return "AttitudeMahonyFilterParameters_time_step_in_seconds_set";
+  case 1683: return "AttitudeMahonyFilterParameters_kp_get";
+  case 1684: return "AttitudeMahonyFilterParameters_kp_set";
+  case 1685: return "AttitudeMahonyFilterParameters_ki_get";
+  case 1686: return "AttitudeMahonyFilterParameters_ki_set";
+  case 1687: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_get";
+  case 1688: return "AttitudeMahonyFilterParameters_use_magnetometer_measurements_set";
+  case 1689: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get";
+  case 1690: return "AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set";
+  case 1691: return "new_AttitudeMahonyFilterParameters";
+  case 1692: return "delete_AttitudeMahonyFilterParameters";
+  case 1693: return "new_AttitudeMahonyFilter";
+  case 1694: return "AttitudeMahonyFilter_useMagnetoMeterMeasurements";
+  case 1695: return "AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements";
+  case 1696: return "AttitudeMahonyFilter_setGainkp";
+  case 1697: return "AttitudeMahonyFilter_setGainki";
+  case 1698: return "AttitudeMahonyFilter_setTimeStepInSeconds";
+  case 1699: return "AttitudeMahonyFilter_setGravityDirection";
+  case 1700: return "AttitudeMahonyFilter_setParameters";
+  case 1701: return "AttitudeMahonyFilter_getParameters";
+  case 1702: return "AttitudeMahonyFilter_updateFilterWithMeasurements";
+  case 1703: return "AttitudeMahonyFilter_propagateStates";
+  case 1704: return "AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix";
+  case 1705: return "AttitudeMahonyFilter_getOrientationEstimateAsQuaternion";
+  case 1706: return "AttitudeMahonyFilter_getOrientationEstimateAsRPY";
+  case 1707: return "AttitudeMahonyFilter_getInternalStateSize";
+  case 1708: return "AttitudeMahonyFilter_getInternalState";
+  case 1709: return "AttitudeMahonyFilter_getDefaultInternalInitialState";
+  case 1710: return "AttitudeMahonyFilter_setInternalState";
+  case 1711: return "AttitudeMahonyFilter_setInternalStateInitialOrientation";
+  case 1712: return "delete_AttitudeMahonyFilter";
+  case 1713: return "DiscreteExtendedKalmanFilterHelper_ekf_f";
+  case 1714: return "DiscreteExtendedKalmanFilterHelper_ekf_h";
+  case 1715: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF";
+  case 1716: return "DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH";
+  case 1717: return "DiscreteExtendedKalmanFilterHelper_ekfPredict";
+  case 1718: return "DiscreteExtendedKalmanFilterHelper_ekfUpdate";
+  case 1719: return "DiscreteExtendedKalmanFilterHelper_ekfInit";
+  case 1720: return "DiscreteExtendedKalmanFilterHelper_ekfReset";
+  case 1721: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector";
+  case 1722: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputVector";
+  case 1723: return "DiscreteExtendedKalmanFilterHelper_ekfSetInitialState";
+  case 1724: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance";
+  case 1725: return "DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance";
+  case 1726: return "DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance";
+  case 1727: return "DiscreteExtendedKalmanFilterHelper_ekfSetStateSize";
+  case 1728: return "DiscreteExtendedKalmanFilterHelper_ekfSetInputSize";
+  case 1729: return "DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize";
+  case 1730: return "DiscreteExtendedKalmanFilterHelper_ekfGetStates";
+  case 1731: return "DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance";
+  case 1732: return "delete_DiscreteExtendedKalmanFilterHelper";
+  case 1733: return "output_dimensions_with_magnetometer_get";
+  case 1734: return "output_dimensions_without_magnetometer_get";
+  case 1735: return "input_dimensions_get";
+  case 1736: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_get";
+  case 1737: return "AttitudeQuaternionEKFParameters_time_step_in_seconds_set";
+  case 1738: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get";
+  case 1739: return "AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set";
+  case 1740: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get";
+  case 1741: return "AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set";
+  case 1742: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get";
+  case 1743: return "AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set";
+  case 1744: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get";
+  case 1745: return "AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set";
+  case 1746: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get";
+  case 1747: return "AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set";
+  case 1748: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get";
+  case 1749: return "AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set";
+  case 1750: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get";
+  case 1751: return "AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set";
+  case 1752: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get";
+  case 1753: return "AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set";
+  case 1754: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get";
+  case 1755: return "AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set";
+  case 1756: return "new_AttitudeQuaternionEKFParameters";
+  case 1757: return "delete_AttitudeQuaternionEKFParameters";
+  case 1758: return "new_AttitudeQuaternionEKF";
+  case 1759: return "AttitudeQuaternionEKF_getParameters";
+  case 1760: return "AttitudeQuaternionEKF_setParameters";
+  case 1761: return "AttitudeQuaternionEKF_setGravityDirection";
+  case 1762: return "AttitudeQuaternionEKF_setTimeStepInSeconds";
+  case 1763: return "AttitudeQuaternionEKF_setBiasCorrelationTimeFactor";
+  case 1764: return "AttitudeQuaternionEKF_useMagnetometerMeasurements";
+  case 1765: return "AttitudeQuaternionEKF_setMeasurementNoiseVariance";
+  case 1766: return "AttitudeQuaternionEKF_setSystemNoiseVariance";
+  case 1767: return "AttitudeQuaternionEKF_setInitialStateCovariance";
+  case 1768: return "AttitudeQuaternionEKF_initializeFilter";
+  case 1769: return "AttitudeQuaternionEKF_updateFilterWithMeasurements";
+  case 1770: return "AttitudeQuaternionEKF_propagateStates";
+  case 1771: return "AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix";
+  case 1772: return "AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion";
+  case 1773: return "AttitudeQuaternionEKF_getOrientationEstimateAsRPY";
+  case 1774: return "AttitudeQuaternionEKF_getInternalStateSize";
+  case 1775: return "AttitudeQuaternionEKF_getInternalState";
+  case 1776: return "AttitudeQuaternionEKF_getDefaultInternalInitialState";
+  case 1777: return "AttitudeQuaternionEKF_setInternalState";
+  case 1778: return "AttitudeQuaternionEKF_setInternalStateInitialOrientation";
+  case 1779: return "delete_AttitudeQuaternionEKF";
+  case 1780: return "_wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass";
+  case 1781: return "DynamicsRegressorParameter_category_get";
+  case 1782: return "DynamicsRegressorParameter_category_set";
+  case 1783: return "DynamicsRegressorParameter_elemIndex_get";
+  case 1784: return "DynamicsRegressorParameter_elemIndex_set";
+  case 1785: return "DynamicsRegressorParameter_type_get";
+  case 1786: return "DynamicsRegressorParameter_type_set";
+  case 1787: return "DynamicsRegressorParameter_lt";
+  case 1788: return "DynamicsRegressorParameter_eq";
+  case 1789: return "DynamicsRegressorParameter_ne";
+  case 1790: return "new_DynamicsRegressorParameter";
+  case 1791: return "delete_DynamicsRegressorParameter";
+  case 1792: return "DynamicsRegressorParametersList_parameters_get";
+  case 1793: return "DynamicsRegressorParametersList_parameters_set";
+  case 1794: return "DynamicsRegressorParametersList_getDescriptionOfParameter";
+  case 1795: return "DynamicsRegressorParametersList_addParam";
+  case 1796: return "DynamicsRegressorParametersList_addList";
+  case 1797: return "DynamicsRegressorParametersList_findParam";
+  case 1798: return "DynamicsRegressorParametersList_getNrOfParameters";
+  case 1799: return "new_DynamicsRegressorParametersList";
+  case 1800: return "delete_DynamicsRegressorParametersList";
+  case 1801: return "new_DynamicsRegressorGenerator";
+  case 1802: return "delete_DynamicsRegressorGenerator";
+  case 1803: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile";
+  case 1804: return "DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString";
+  case 1805: return "DynamicsRegressorGenerator_loadRegressorStructureFromFile";
+  case 1806: return "DynamicsRegressorGenerator_loadRegressorStructureFromString";
+  case 1807: return "DynamicsRegressorGenerator_isValid";
+  case 1808: return "DynamicsRegressorGenerator_getNrOfParameters";
+  case 1809: return "DynamicsRegressorGenerator_getNrOfOutputs";
+  case 1810: return "DynamicsRegressorGenerator_getNrOfDegreesOfFreedom";
+  case 1811: return "DynamicsRegressorGenerator_getDescriptionOfParameter";
+  case 1812: return "DynamicsRegressorGenerator_getDescriptionOfParameters";
+  case 1813: return "DynamicsRegressorGenerator_getDescriptionOfOutput";
+  case 1814: return "DynamicsRegressorGenerator_getDescriptionOfOutputs";
+  case 1815: return "DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom";
+  case 1816: return "DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom";
+  case 1817: return "DynamicsRegressorGenerator_getDescriptionOfLink";
+  case 1818: return "DynamicsRegressorGenerator_getDescriptionOfLinks";
+  case 1819: return "DynamicsRegressorGenerator_getNrOfLinks";
+  case 1820: return "DynamicsRegressorGenerator_getNrOfFakeLinks";
+  case 1821: return "DynamicsRegressorGenerator_getBaseLinkName";
+  case 1822: return "DynamicsRegressorGenerator_getSensorsModel";
+  case 1823: return "DynamicsRegressorGenerator_setRobotState";
+  case 1824: return "DynamicsRegressorGenerator_getSensorsMeasurements";
+  case 1825: return "DynamicsRegressorGenerator_setTorqueSensorMeasurement";
+  case 1826: return "DynamicsRegressorGenerator_computeRegressor";
+  case 1827: return "DynamicsRegressorGenerator_getModelParameters";
+  case 1828: return "DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace";
+  case 1829: return "DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace";
+  case 1830: return "DynamicsRegressorGenerator_generate_random_regressors";
+  case 1831: return "new_KinDynComputations";
+  case 1832: return "delete_KinDynComputations";
+  case 1833: return "KinDynComputations_loadRobotModel";
+  case 1834: return "KinDynComputations_loadRobotModelFromFile";
+  case 1835: return "KinDynComputations_loadRobotModelFromString";
+  case 1836: return "KinDynComputations_isValid";
+  case 1837: return "KinDynComputations_setFrameVelocityRepresentation";
+  case 1838: return "KinDynComputations_getFrameVelocityRepresentation";
+  case 1839: return "KinDynComputations_getNrOfDegreesOfFreedom";
+  case 1840: return "KinDynComputations_getDescriptionOfDegreeOfFreedom";
+  case 1841: return "KinDynComputations_getDescriptionOfDegreesOfFreedom";
+  case 1842: return "KinDynComputations_getNrOfLinks";
+  case 1843: return "KinDynComputations_getNrOfFrames";
+  case 1844: return "KinDynComputations_getFloatingBase";
+  case 1845: return "KinDynComputations_setFloatingBase";
+  case 1846: return "KinDynComputations_model";
+  case 1847: return "KinDynComputations_getRobotModel";
+  case 1848: return "KinDynComputations_getRelativeJacobianSparsityPattern";
+  case 1849: return "KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern";
+  case 1850: return "KinDynComputations_setJointPos";
+  case 1851: return "KinDynComputations_setRobotState";
+  case 1852: return "KinDynComputations_getRobotState";
+  case 1853: return "KinDynComputations_getWorldBaseTransform";
+  case 1854: return "KinDynComputations_getBaseTwist";
+  case 1855: return "KinDynComputations_getJointPos";
+  case 1856: return "KinDynComputations_getJointVel";
+  case 1857: return "KinDynComputations_getModelVel";
+  case 1858: return "KinDynComputations_getFrameIndex";
+  case 1859: return "KinDynComputations_getFrameName";
+  case 1860: return "KinDynComputations_getWorldTransform";
+  case 1861: return "KinDynComputations_getRelativeTransformExplicit";
+  case 1862: return "KinDynComputations_getRelativeTransform";
+  case 1863: return "KinDynComputations_getFrameVel";
+  case 1864: return "KinDynComputations_getFrameAcc";
+  case 1865: return "KinDynComputations_getFrameFreeFloatingJacobian";
+  case 1866: return "KinDynComputations_getRelativeJacobian";
+  case 1867: return "KinDynComputations_getRelativeJacobianExplicit";
+  case 1868: return "KinDynComputations_getFrameBiasAcc";
+  case 1869: return "KinDynComputations_getCenterOfMassPosition";
+  case 1870: return "KinDynComputations_getCenterOfMassVelocity";
+  case 1871: return "KinDynComputations_getCenterOfMassJacobian";
+  case 1872: return "KinDynComputations_getCenterOfMassBiasAcc";
+  case 1873: return "KinDynComputations_getAverageVelocity";
+  case 1874: return "KinDynComputations_getAverageVelocityJacobian";
+  case 1875: return "KinDynComputations_getCentroidalAverageVelocity";
+  case 1876: return "KinDynComputations_getCentroidalAverageVelocityJacobian";
+  case 1877: return "KinDynComputations_getLinearAngularMomentum";
+  case 1878: return "KinDynComputations_getLinearAngularMomentumJacobian";
+  case 1879: return "KinDynComputations_getCentroidalTotalMomentum";
+  case 1880: return "KinDynComputations_getFreeFloatingMassMatrix";
+  case 1881: return "KinDynComputations_inverseDynamics";
+  case 1882: return "KinDynComputations_generalizedBiasForces";
+  case 1883: return "KinDynComputations_generalizedGravityForces";
+  case 1884: return "KinDynComputations_generalizedExternalForces";
+  case 1885: return "KinDynComputations_inverseDynamicsInertialParametersRegressor";
+  case 1886: return "delete_ICamera";
+  case 1887: return "ICamera_setPosition";
+  case 1888: return "ICamera_setTarget";
+  case 1889: return "ICamera_setUpVector";
+  case 1890: return "ColorViz_r_get";
+  case 1891: return "ColorViz_r_set";
+  case 1892: return "ColorViz_g_get";
+  case 1893: return "ColorViz_g_set";
+  case 1894: return "ColorViz_b_get";
+  case 1895: return "ColorViz_b_set";
+  case 1896: return "ColorViz_a_get";
+  case 1897: return "ColorViz_a_set";
+  case 1898: return "new_ColorViz";
+  case 1899: return "delete_ColorViz";
+  case 1900: return "delete_ILight";
+  case 1901: return "ILight_getName";
+  case 1902: return "ILight_setType";
+  case 1903: return "ILight_getType";
+  case 1904: return "ILight_setPosition";
+  case 1905: return "ILight_getPosition";
+  case 1906: return "ILight_setDirection";
+  case 1907: return "ILight_getDirection";
+  case 1908: return "ILight_setAmbientColor";
+  case 1909: return "ILight_getAmbientColor";
+  case 1910: return "ILight_setSpecularColor";
+  case 1911: return "ILight_getSpecularColor";
+  case 1912: return "ILight_setDiffuseColor";
+  case 1913: return "ILight_getDiffuseColor";
+  case 1914: return "delete_IEnvironment";
+  case 1915: return "IEnvironment_getElements";
+  case 1916: return "IEnvironment_setElementVisibility";
+  case 1917: return "IEnvironment_setBackgroundColor";
+  case 1918: return "IEnvironment_setAmbientLight";
+  case 1919: return "IEnvironment_getLights";
+  case 1920: return "IEnvironment_addLight";
+  case 1921: return "IEnvironment_lightViz";
+  case 1922: return "IEnvironment_removeLight";
+  case 1923: return "delete_IJetsVisualization";
+  case 1924: return "IJetsVisualization_setJetsFrames";
+  case 1925: return "IJetsVisualization_getNrOfJets";
+  case 1926: return "IJetsVisualization_getJetDirection";
+  case 1927: return "IJetsVisualization_setJetDirection";
+  case 1928: return "IJetsVisualization_setJetColor";
+  case 1929: return "IJetsVisualization_setJetsDimensions";
+  case 1930: return "IJetsVisualization_setJetsIntensity";
+  case 1931: return "delete_IVectorsVisualization";
+  case 1932: return "IVectorsVisualization_addVector";
+  case 1933: return "IVectorsVisualization_getNrOfVectors";
+  case 1934: return "IVectorsVisualization_getVector";
+  case 1935: return "IVectorsVisualization_updateVector";
+  case 1936: return "IVectorsVisualization_setVectorColor";
+  case 1937: return "IVectorsVisualization_setVectorsAspect";
+  case 1938: return "delete_IModelVisualization";
+  case 1939: return "IModelVisualization_setPositions";
+  case 1940: return "IModelVisualization_setLinkPositions";
+  case 1941: return "IModelVisualization_model";
+  case 1942: return "IModelVisualization_getInstanceName";
+  case 1943: return "IModelVisualization_setModelVisibility";
+  case 1944: return "IModelVisualization_setModelColor";
+  case 1945: return "IModelVisualization_resetModelColor";
+  case 1946: return "IModelVisualization_setLinkColor";
+  case 1947: return "IModelVisualization_resetLinkColor";
+  case 1948: return "IModelVisualization_getLinkNames";
+  case 1949: return "IModelVisualization_setLinkVisibility";
+  case 1950: return "IModelVisualization_getFeatures";
+  case 1951: return "IModelVisualization_setFeatureVisibility";
+  case 1952: return "IModelVisualization_jets";
+  case 1953: return "IModelVisualization_getWorldModelTransform";
+  case 1954: return "IModelVisualization_getWorldLinkTransform";
+  case 1955: return "VisualizerOptions_verbose_get";
+  case 1956: return "VisualizerOptions_verbose_set";
+  case 1957: return "VisualizerOptions_winWidth_get";
+  case 1958: return "VisualizerOptions_winWidth_set";
+  case 1959: return "VisualizerOptions_winHeight_get";
+  case 1960: return "VisualizerOptions_winHeight_set";
+  case 1961: return "VisualizerOptions_rootFrameArrowsDimension_get";
+  case 1962: return "VisualizerOptions_rootFrameArrowsDimension_set";
+  case 1963: return "new_VisualizerOptions";
+  case 1964: return "delete_VisualizerOptions";
+  case 1965: return "new_Visualizer";
+  case 1966: return "delete_Visualizer";
+  case 1967: return "Visualizer_init";
+  case 1968: return "Visualizer_getNrOfVisualizedModels";
+  case 1969: return "Visualizer_getModelInstanceName";
+  case 1970: return "Visualizer_getModelInstanceIndex";
+  case 1971: return "Visualizer_addModel";
+  case 1972: return "Visualizer_modelViz";
+  case 1973: return "Visualizer_camera";
+  case 1974: return "Visualizer_enviroment";
+  case 1975: return "Visualizer_vectors";
+  case 1976: return "Visualizer_run";
+  case 1977: return "Visualizer_draw";
+  case 1978: return "Visualizer_drawToFile";
+  case 1979: return "Visualizer_close";
+  case 1980: return "new_DynamicsComputations";
+  case 1981: return "delete_DynamicsComputations";
+  case 1982: return "DynamicsComputations_loadRobotModelFromFile";
+  case 1983: return "DynamicsComputations_loadRobotModelFromString";
+  case 1984: return "DynamicsComputations_isValid";
+  case 1985: return "DynamicsComputations_getNrOfDegreesOfFreedom";
+  case 1986: return "DynamicsComputations_getDescriptionOfDegreeOfFreedom";
+  case 1987: return "DynamicsComputations_getDescriptionOfDegreesOfFreedom";
+  case 1988: return "DynamicsComputations_getNrOfLinks";
+  case 1989: return "DynamicsComputations_getNrOfFrames";
+  case 1990: return "DynamicsComputations_getFloatingBase";
+  case 1991: return "DynamicsComputations_setFloatingBase";
+  case 1992: return "DynamicsComputations_setRobotState";
+  case 1993: return "DynamicsComputations_getWorldBaseTransform";
+  case 1994: return "DynamicsComputations_getBaseTwist";
+  case 1995: return "DynamicsComputations_getJointPos";
+  case 1996: return "DynamicsComputations_getJointVel";
+  case 1997: return "DynamicsComputations_getFrameIndex";
+  case 1998: return "DynamicsComputations_getFrameName";
+  case 1999: return "DynamicsComputations_getWorldTransform";
+  case 2000: return "DynamicsComputations_getRelativeTransform";
+  case 2001: return "DynamicsComputations_getFrameTwist";
+  case 2002: return "DynamicsComputations_getFrameTwistInWorldOrient";
+  case 2003: return "DynamicsComputations_getFrameProperSpatialAcceleration";
+  case 2004: return "DynamicsComputations_getLinkIndex";
+  case 2005: return "DynamicsComputations_getLinkInertia";
+  case 2006: return "DynamicsComputations_getJointIndex";
+  case 2007: return "DynamicsComputations_getJointName";
+  case 2008: return "DynamicsComputations_getJointLimits";
+  case 2009: return "DynamicsComputations_inverseDynamics";
+  case 2010: return "DynamicsComputations_getFreeFloatingMassMatrix";
+  case 2011: return "DynamicsComputations_getFrameJacobian";
+  case 2012: return "DynamicsComputations_getDynamicsRegressor";
+  case 2013: return "DynamicsComputations_getModelDynamicsParameters";
+  case 2014: return "DynamicsComputations_getCenterOfMass";
+  case 2015: return "DynamicsComputations_getCenterOfMassJacobian";
   default: return 0;
   }
 }
@@ -100541,523 +101454,532 @@ void mexFunction(int resc, mxArray *resv[], int argc, const mxArray *argv[]) {
   case 1487: flag=_wrap_ModelExporter_isValid(resc,resv,argc,(mxArray**)(argv)); break;
   case 1488: flag=_wrap_ModelExporter_exportModelToString(resc,resv,argc,(mxArray**)(argv)); break;
   case 1489: flag=_wrap_ModelExporter_exportModelToFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1490: flag=_wrap_new_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1491: flag=_wrap_UnknownWrenchContact_unknownType_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1492: flag=_wrap_UnknownWrenchContact_unknownType_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1493: flag=_wrap_UnknownWrenchContact_contactPoint_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1494: flag=_wrap_UnknownWrenchContact_contactPoint_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1495: flag=_wrap_UnknownWrenchContact_forceDirection_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1496: flag=_wrap_UnknownWrenchContact_forceDirection_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1497: flag=_wrap_UnknownWrenchContact_knownWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1498: flag=_wrap_UnknownWrenchContact_knownWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1499: flag=_wrap_UnknownWrenchContact_contactId_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1500: flag=_wrap_UnknownWrenchContact_contactId_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1501: flag=_wrap_delete_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1502: flag=_wrap_new_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1503: flag=_wrap_LinkUnknownWrenchContacts_clear(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1504: flag=_wrap_LinkUnknownWrenchContacts_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1505: flag=_wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1506: flag=_wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1507: flag=_wrap_LinkUnknownWrenchContacts_addNewContactForLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1508: flag=_wrap_LinkUnknownWrenchContacts_addNewContactInFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1509: flag=_wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1510: flag=_wrap_LinkUnknownWrenchContacts_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1511: flag=_wrap_LinkUnknownWrenchContacts_toString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1512: flag=_wrap_delete_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1513: flag=_wrap_new_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1514: flag=_wrap_estimateExternalWrenchesBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1515: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1516: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1517: flag=_wrap_estimateExternalWrenchesBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1518: flag=_wrap_estimateExternalWrenchesBuffers_A_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1519: flag=_wrap_estimateExternalWrenchesBuffers_A_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1520: flag=_wrap_estimateExternalWrenchesBuffers_x_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1521: flag=_wrap_estimateExternalWrenchesBuffers_x_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1522: flag=_wrap_estimateExternalWrenchesBuffers_b_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1523: flag=_wrap_estimateExternalWrenchesBuffers_b_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1524: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1525: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1526: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1527: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1528: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1529: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1530: flag=_wrap_delete_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1531: flag=_wrap_estimateExternalWrenchesWithoutInternalFT(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1532: flag=_wrap_estimateExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1533: flag=_wrap_dynamicsEstimationForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1534: flag=_wrap_dynamicsEstimationForwardVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1535: flag=_wrap_computeLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1536: flag=_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1537: flag=_wrap_new_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1538: flag=_wrap_delete_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1539: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1540: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1541: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1542: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1543: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1544: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_submodels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1545: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1546: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1547: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1548: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1549: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1550: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1551: flag=_wrap_new_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1552: flag=_wrap_delete_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1553: flag=_wrap_SimpleLeggedOdometry_setModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1554: flag=_wrap_SimpleLeggedOdometry_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1555: flag=_wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1556: flag=_wrap_SimpleLeggedOdometry_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1557: flag=_wrap_SimpleLeggedOdometry_updateKinematics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1558: flag=_wrap_SimpleLeggedOdometry_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1559: flag=_wrap_SimpleLeggedOdometry_changeFixedFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1560: flag=_wrap_SimpleLeggedOdometry_getCurrentFixedLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1561: flag=_wrap_SimpleLeggedOdometry_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1562: flag=_wrap_SimpleLeggedOdometry_getWorldFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1563: flag=_wrap_isLinkBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1564: flag=_wrap_isJointBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1565: flag=_wrap_isDOFBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1566: flag=_wrap_new_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1567: flag=_wrap_BerdyOptions_berdyVariant_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1568: flag=_wrap_BerdyOptions_berdyVariant_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1569: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1570: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1571: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1572: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1573: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1574: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1575: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1576: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1577: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1578: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1579: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1580: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1581: flag=_wrap_BerdyOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1582: flag=_wrap_BerdyOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1583: flag=_wrap_BerdyOptions_checkConsistency(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1584: flag=_wrap_delete_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1585: flag=_wrap_BerdySensor_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1586: flag=_wrap_BerdySensor_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1587: flag=_wrap_BerdySensor_id_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1588: flag=_wrap_BerdySensor_id_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1589: flag=_wrap_BerdySensor_range_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1590: flag=_wrap_BerdySensor_range_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1591: flag=_wrap_BerdySensor_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1592: flag=_wrap_BerdySensor_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1593: flag=_wrap_new_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1594: flag=_wrap_delete_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1595: flag=_wrap_BerdyDynamicVariable_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1596: flag=_wrap_BerdyDynamicVariable_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1597: flag=_wrap_BerdyDynamicVariable_id_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1598: flag=_wrap_BerdyDynamicVariable_id_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1599: flag=_wrap_BerdyDynamicVariable_range_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1600: flag=_wrap_BerdyDynamicVariable_range_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1601: flag=_wrap_BerdyDynamicVariable_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1602: flag=_wrap_BerdyDynamicVariable_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1603: flag=_wrap_new_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1604: flag=_wrap_delete_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1605: flag=_wrap_new_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1606: flag=_wrap_BerdyHelper_dynamicTraversal(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1607: flag=_wrap_BerdyHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1608: flag=_wrap_BerdyHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1609: flag=_wrap_BerdyHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1610: flag=_wrap_BerdyHelper_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1611: flag=_wrap_BerdyHelper_getOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1612: flag=_wrap_BerdyHelper_getNrOfDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1613: flag=_wrap_BerdyHelper_getNrOfDynamicEquations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1614: flag=_wrap_BerdyHelper_getNrOfSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1615: flag=_wrap_BerdyHelper_resizeAndZeroBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1616: flag=_wrap_BerdyHelper_getBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1617: flag=_wrap_BerdyHelper_getSensorsOrdering(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1618: flag=_wrap_BerdyHelper_getRangeSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1619: flag=_wrap_BerdyHelper_getRangeDOFSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1620: flag=_wrap_BerdyHelper_getRangeJointSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1621: flag=_wrap_BerdyHelper_getRangeLinkSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1622: flag=_wrap_BerdyHelper_getRangeLinkVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1623: flag=_wrap_BerdyHelper_getRangeJointVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1624: flag=_wrap_BerdyHelper_getRangeDOFVariable(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1625: flag=_wrap_BerdyHelper_getDynamicVariablesOrdering(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1626: flag=_wrap_BerdyHelper_serializeDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1627: flag=_wrap_BerdyHelper_serializeSensorVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1628: flag=_wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1629: flag=_wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1630: flag=_wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1631: flag=_wrap_BerdyHelper_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1632: flag=_wrap_BerdyHelper_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1633: flag=_wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1634: flag=_wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1635: flag=_wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1636: flag=_wrap_delete_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1637: flag=_wrap_new_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1638: flag=_wrap_delete_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1639: flag=_wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1640: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1641: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1642: flag=_wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1643: flag=_wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1644: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1645: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1646: flag=_wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1647: flag=_wrap_BerdySparseMAPSolver_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1648: flag=_wrap_BerdySparseMAPSolver_initialize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1649: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1650: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1651: flag=_wrap_BerdySparseMAPSolver_doEstimate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1652: flag=_wrap_BerdySparseMAPSolver_getLastEstimate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1653: flag=_wrap_AttitudeEstimatorState_m_orientation_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1654: flag=_wrap_AttitudeEstimatorState_m_orientation_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1655: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1656: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1657: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1658: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1659: flag=_wrap_new_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1660: flag=_wrap_delete_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1661: flag=_wrap_delete_IAttitudeEstimator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1662: flag=_wrap_IAttitudeEstimator_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1663: flag=_wrap_IAttitudeEstimator_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1664: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1665: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1666: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1667: flag=_wrap_IAttitudeEstimator_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1668: flag=_wrap_IAttitudeEstimator_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1669: flag=_wrap_IAttitudeEstimator_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1670: flag=_wrap_IAttitudeEstimator_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1671: flag=_wrap_IAttitudeEstimator_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1672: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1673: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1674: flag=_wrap_AttitudeMahonyFilterParameters_kp_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1675: flag=_wrap_AttitudeMahonyFilterParameters_kp_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1676: flag=_wrap_AttitudeMahonyFilterParameters_ki_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1677: flag=_wrap_AttitudeMahonyFilterParameters_ki_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1678: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1679: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1680: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1681: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1682: flag=_wrap_new_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1683: flag=_wrap_delete_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1684: flag=_wrap_new_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1685: flag=_wrap_AttitudeMahonyFilter_useMagnetoMeterMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1686: flag=_wrap_AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1687: flag=_wrap_AttitudeMahonyFilter_setGainkp(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1688: flag=_wrap_AttitudeMahonyFilter_setGainki(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1689: flag=_wrap_AttitudeMahonyFilter_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1690: flag=_wrap_AttitudeMahonyFilter_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1691: flag=_wrap_AttitudeMahonyFilter_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1692: flag=_wrap_AttitudeMahonyFilter_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1693: flag=_wrap_AttitudeMahonyFilter_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1694: flag=_wrap_AttitudeMahonyFilter_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1695: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1696: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1697: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1698: flag=_wrap_AttitudeMahonyFilter_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1699: flag=_wrap_AttitudeMahonyFilter_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1700: flag=_wrap_AttitudeMahonyFilter_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1701: flag=_wrap_AttitudeMahonyFilter_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1702: flag=_wrap_AttitudeMahonyFilter_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1703: flag=_wrap_delete_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1704: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_f(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1705: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1706: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1707: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1708: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfPredict(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1709: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1710: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1711: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1712: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1713: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1714: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1715: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1716: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1717: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1718: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1719: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1720: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1721: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1722: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1723: flag=_wrap_delete_DiscreteExtendedKalmanFilterHelper(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1724: flag=_wrap_output_dimensions_with_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1725: flag=_wrap_output_dimensions_without_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1726: flag=_wrap_input_dimensions_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1727: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1728: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1729: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1730: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1731: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1732: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1733: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1734: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1735: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1736: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1737: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1738: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1739: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1740: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1741: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1742: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1743: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1744: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1745: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1746: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1747: flag=_wrap_new_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1748: flag=_wrap_delete_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1749: flag=_wrap_new_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1750: flag=_wrap_AttitudeQuaternionEKF_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1751: flag=_wrap_AttitudeQuaternionEKF_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1752: flag=_wrap_AttitudeQuaternionEKF_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1753: flag=_wrap_AttitudeQuaternionEKF_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1754: flag=_wrap_AttitudeQuaternionEKF_setBiasCorrelationTimeFactor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1755: flag=_wrap_AttitudeQuaternionEKF_useMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1756: flag=_wrap_AttitudeQuaternionEKF_setMeasurementNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1757: flag=_wrap_AttitudeQuaternionEKF_setSystemNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1758: flag=_wrap_AttitudeQuaternionEKF_setInitialStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1759: flag=_wrap_AttitudeQuaternionEKF_initializeFilter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1760: flag=_wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1761: flag=_wrap_AttitudeQuaternionEKF_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1762: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1763: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1764: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1765: flag=_wrap_AttitudeQuaternionEKF_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1766: flag=_wrap_AttitudeQuaternionEKF_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1767: flag=_wrap_AttitudeQuaternionEKF_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1768: flag=_wrap_AttitudeQuaternionEKF_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1769: flag=_wrap_AttitudeQuaternionEKF_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1770: flag=_wrap_delete_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1771: flag=_wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1772: flag=_wrap_DynamicsRegressorParameter_category_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1773: flag=_wrap_DynamicsRegressorParameter_category_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1774: flag=_wrap_DynamicsRegressorParameter_elemIndex_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1775: flag=_wrap_DynamicsRegressorParameter_elemIndex_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1776: flag=_wrap_DynamicsRegressorParameter_type_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1777: flag=_wrap_DynamicsRegressorParameter_type_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1778: flag=_wrap_DynamicsRegressorParameter_lt(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1779: flag=_wrap_DynamicsRegressorParameter_eq(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1780: flag=_wrap_DynamicsRegressorParameter_ne(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1781: flag=_wrap_new_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1782: flag=_wrap_delete_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1783: flag=_wrap_DynamicsRegressorParametersList_parameters_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1784: flag=_wrap_DynamicsRegressorParametersList_parameters_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1785: flag=_wrap_DynamicsRegressorParametersList_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1786: flag=_wrap_DynamicsRegressorParametersList_addParam(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1787: flag=_wrap_DynamicsRegressorParametersList_addList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1788: flag=_wrap_DynamicsRegressorParametersList_findParam(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1789: flag=_wrap_DynamicsRegressorParametersList_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1790: flag=_wrap_new_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1791: flag=_wrap_delete_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1792: flag=_wrap_new_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1793: flag=_wrap_delete_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1794: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1795: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1796: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1797: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1798: flag=_wrap_DynamicsRegressorGenerator_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1799: flag=_wrap_DynamicsRegressorGenerator_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1800: flag=_wrap_DynamicsRegressorGenerator_getNrOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1801: flag=_wrap_DynamicsRegressorGenerator_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1802: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1803: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1804: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutput(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1805: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1806: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1807: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1808: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLink(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1809: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1810: flag=_wrap_DynamicsRegressorGenerator_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1811: flag=_wrap_DynamicsRegressorGenerator_getNrOfFakeLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1812: flag=_wrap_DynamicsRegressorGenerator_getBaseLinkName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1813: flag=_wrap_DynamicsRegressorGenerator_getSensorsModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1814: flag=_wrap_DynamicsRegressorGenerator_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1815: flag=_wrap_DynamicsRegressorGenerator_getSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1816: flag=_wrap_DynamicsRegressorGenerator_setTorqueSensorMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1817: flag=_wrap_DynamicsRegressorGenerator_computeRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1818: flag=_wrap_DynamicsRegressorGenerator_getModelParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1819: flag=_wrap_DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1820: flag=_wrap_DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1821: flag=_wrap_DynamicsRegressorGenerator_generate_random_regressors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1822: flag=_wrap_new_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1823: flag=_wrap_delete_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1824: flag=_wrap_KinDynComputations_loadRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1825: flag=_wrap_KinDynComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1826: flag=_wrap_KinDynComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1827: flag=_wrap_KinDynComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1828: flag=_wrap_KinDynComputations_setFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1829: flag=_wrap_KinDynComputations_getFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1830: flag=_wrap_KinDynComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1831: flag=_wrap_KinDynComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1832: flag=_wrap_KinDynComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1833: flag=_wrap_KinDynComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1834: flag=_wrap_KinDynComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1835: flag=_wrap_KinDynComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1836: flag=_wrap_KinDynComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1837: flag=_wrap_KinDynComputations_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1838: flag=_wrap_KinDynComputations_getRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1839: flag=_wrap_KinDynComputations_getRelativeJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1840: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1841: flag=_wrap_KinDynComputations_setJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1842: flag=_wrap_KinDynComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1843: flag=_wrap_KinDynComputations_getRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1844: flag=_wrap_KinDynComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1845: flag=_wrap_KinDynComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1846: flag=_wrap_KinDynComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1847: flag=_wrap_KinDynComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1848: flag=_wrap_KinDynComputations_getModelVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1849: flag=_wrap_KinDynComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1850: flag=_wrap_KinDynComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1851: flag=_wrap_KinDynComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1852: flag=_wrap_KinDynComputations_getRelativeTransformExplicit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1853: flag=_wrap_KinDynComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1854: flag=_wrap_KinDynComputations_getFrameVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1855: flag=_wrap_KinDynComputations_getFrameAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1856: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1857: flag=_wrap_KinDynComputations_getRelativeJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1858: flag=_wrap_KinDynComputations_getRelativeJacobianExplicit(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1859: flag=_wrap_KinDynComputations_getFrameBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1860: flag=_wrap_KinDynComputations_getCenterOfMassPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1861: flag=_wrap_KinDynComputations_getCenterOfMassVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1862: flag=_wrap_KinDynComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1863: flag=_wrap_KinDynComputations_getCenterOfMassBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1864: flag=_wrap_KinDynComputations_getAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1865: flag=_wrap_KinDynComputations_getAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1866: flag=_wrap_KinDynComputations_getCentroidalAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1867: flag=_wrap_KinDynComputations_getCentroidalAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1868: flag=_wrap_KinDynComputations_getLinearAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1869: flag=_wrap_KinDynComputations_getLinearAngularMomentumJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1870: flag=_wrap_KinDynComputations_getCentroidalTotalMomentum(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1871: flag=_wrap_KinDynComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1872: flag=_wrap_KinDynComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1873: flag=_wrap_KinDynComputations_generalizedBiasForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1874: flag=_wrap_KinDynComputations_generalizedGravityForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1875: flag=_wrap_KinDynComputations_generalizedExternalForces(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1876: flag=_wrap_KinDynComputations_inverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1877: flag=_wrap_delete_ICamera(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1878: flag=_wrap_ICamera_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1879: flag=_wrap_ICamera_setTarget(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1880: flag=_wrap_ICamera_setUpVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1881: flag=_wrap_ColorViz_r_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1882: flag=_wrap_ColorViz_r_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1883: flag=_wrap_ColorViz_g_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1884: flag=_wrap_ColorViz_g_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1885: flag=_wrap_ColorViz_b_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1886: flag=_wrap_ColorViz_b_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1887: flag=_wrap_ColorViz_a_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1888: flag=_wrap_ColorViz_a_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1889: flag=_wrap_new_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1890: flag=_wrap_delete_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1891: flag=_wrap_delete_ILight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1892: flag=_wrap_ILight_getName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1893: flag=_wrap_ILight_setType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1894: flag=_wrap_ILight_getType(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1895: flag=_wrap_ILight_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1896: flag=_wrap_ILight_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1897: flag=_wrap_ILight_setDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1898: flag=_wrap_ILight_getDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1899: flag=_wrap_ILight_setAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1900: flag=_wrap_ILight_getAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1901: flag=_wrap_ILight_setSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1902: flag=_wrap_ILight_getSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1903: flag=_wrap_ILight_setDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1904: flag=_wrap_ILight_getDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1905: flag=_wrap_delete_IEnvironment(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1906: flag=_wrap_IEnvironment_getElements(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1907: flag=_wrap_IEnvironment_setElementVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1908: flag=_wrap_IEnvironment_setBackgroundColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1909: flag=_wrap_IEnvironment_setAmbientLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1910: flag=_wrap_IEnvironment_getLights(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1911: flag=_wrap_IEnvironment_addLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1912: flag=_wrap_IEnvironment_lightViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1913: flag=_wrap_IEnvironment_removeLight(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1914: flag=_wrap_delete_IJetsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1915: flag=_wrap_IJetsVisualization_setJetsFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1916: flag=_wrap_IJetsVisualization_getNrOfJets(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1917: flag=_wrap_IJetsVisualization_getJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1918: flag=_wrap_IJetsVisualization_setJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1919: flag=_wrap_IJetsVisualization_setJetColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1920: flag=_wrap_IJetsVisualization_setJetsDimensions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1921: flag=_wrap_IJetsVisualization_setJetsIntensity(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1922: flag=_wrap_delete_IVectorsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1923: flag=_wrap_IVectorsVisualization_addVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1924: flag=_wrap_IVectorsVisualization_getNrOfVectors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1925: flag=_wrap_IVectorsVisualization_getVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1926: flag=_wrap_IVectorsVisualization_updateVector(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1927: flag=_wrap_IVectorsVisualization_setVectorColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1928: flag=_wrap_IVectorsVisualization_setVectorsAspect(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1929: flag=_wrap_delete_IModelVisualization(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1930: flag=_wrap_IModelVisualization_setPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1931: flag=_wrap_IModelVisualization_setLinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1932: flag=_wrap_IModelVisualization_model(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1933: flag=_wrap_IModelVisualization_getInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1934: flag=_wrap_IModelVisualization_setModelVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1935: flag=_wrap_IModelVisualization_setModelColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1936: flag=_wrap_IModelVisualization_resetModelColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1937: flag=_wrap_IModelVisualization_setLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1938: flag=_wrap_IModelVisualization_resetLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1939: flag=_wrap_IModelVisualization_getLinkNames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1940: flag=_wrap_IModelVisualization_setLinkVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1941: flag=_wrap_IModelVisualization_getFeatures(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1942: flag=_wrap_IModelVisualization_setFeatureVisibility(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1943: flag=_wrap_IModelVisualization_jets(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1944: flag=_wrap_IModelVisualization_getWorldModelTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1945: flag=_wrap_IModelVisualization_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1946: flag=_wrap_VisualizerOptions_verbose_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1947: flag=_wrap_VisualizerOptions_verbose_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1948: flag=_wrap_VisualizerOptions_winWidth_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1949: flag=_wrap_VisualizerOptions_winWidth_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1950: flag=_wrap_VisualizerOptions_winHeight_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1951: flag=_wrap_VisualizerOptions_winHeight_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1952: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_get(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1953: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_set(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1954: flag=_wrap_new_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1955: flag=_wrap_delete_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1956: flag=_wrap_new_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1957: flag=_wrap_delete_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1958: flag=_wrap_Visualizer_init(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1959: flag=_wrap_Visualizer_getNrOfVisualizedModels(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1960: flag=_wrap_Visualizer_getModelInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1961: flag=_wrap_Visualizer_getModelInstanceIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1962: flag=_wrap_Visualizer_addModel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1963: flag=_wrap_Visualizer_modelViz(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1964: flag=_wrap_Visualizer_camera(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1965: flag=_wrap_Visualizer_enviroment(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1966: flag=_wrap_Visualizer_vectors(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1967: flag=_wrap_Visualizer_run(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1968: flag=_wrap_Visualizer_draw(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1969: flag=_wrap_Visualizer_drawToFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1970: flag=_wrap_Visualizer_close(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1971: flag=_wrap_new_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1972: flag=_wrap_delete_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1973: flag=_wrap_DynamicsComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1974: flag=_wrap_DynamicsComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1975: flag=_wrap_DynamicsComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1976: flag=_wrap_DynamicsComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1977: flag=_wrap_DynamicsComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1978: flag=_wrap_DynamicsComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1979: flag=_wrap_DynamicsComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1980: flag=_wrap_DynamicsComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1981: flag=_wrap_DynamicsComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1982: flag=_wrap_DynamicsComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1983: flag=_wrap_DynamicsComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1984: flag=_wrap_DynamicsComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1985: flag=_wrap_DynamicsComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1986: flag=_wrap_DynamicsComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1987: flag=_wrap_DynamicsComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1988: flag=_wrap_DynamicsComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1989: flag=_wrap_DynamicsComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1990: flag=_wrap_DynamicsComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1991: flag=_wrap_DynamicsComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1992: flag=_wrap_DynamicsComputations_getFrameTwist(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1993: flag=_wrap_DynamicsComputations_getFrameTwistInWorldOrient(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1994: flag=_wrap_DynamicsComputations_getFrameProperSpatialAcceleration(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1995: flag=_wrap_DynamicsComputations_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1996: flag=_wrap_DynamicsComputations_getLinkInertia(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1997: flag=_wrap_DynamicsComputations_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1998: flag=_wrap_DynamicsComputations_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
-  case 1999: flag=_wrap_DynamicsComputations_getJointLimits(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2000: flag=_wrap_DynamicsComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2001: flag=_wrap_DynamicsComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2002: flag=_wrap_DynamicsComputations_getFrameJacobian(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2003: flag=_wrap_DynamicsComputations_getDynamicsRegressor(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2004: flag=_wrap_DynamicsComputations_getModelDynamicsParameters(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2005: flag=_wrap_DynamicsComputations_getCenterOfMass(resc,resv,argc,(mxArray**)(argv)); break;
-  case 2006: flag=_wrap_DynamicsComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1490: flag=_wrap_new_ModelCalibrationHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1491: flag=_wrap_delete_ModelCalibrationHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1492: flag=_wrap_ModelCalibrationHelper_loadModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1493: flag=_wrap_ModelCalibrationHelper_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1494: flag=_wrap_ModelCalibrationHelper_updateModelInertialParametersToString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1495: flag=_wrap_ModelCalibrationHelper_updateModelInertialParametersToFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1496: flag=_wrap_ModelCalibrationHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1497: flag=_wrap_ModelCalibrationHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1498: flag=_wrap_ModelCalibrationHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1499: flag=_wrap_new_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1500: flag=_wrap_UnknownWrenchContact_unknownType_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1501: flag=_wrap_UnknownWrenchContact_unknownType_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1502: flag=_wrap_UnknownWrenchContact_contactPoint_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1503: flag=_wrap_UnknownWrenchContact_contactPoint_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1504: flag=_wrap_UnknownWrenchContact_forceDirection_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1505: flag=_wrap_UnknownWrenchContact_forceDirection_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1506: flag=_wrap_UnknownWrenchContact_knownWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1507: flag=_wrap_UnknownWrenchContact_knownWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1508: flag=_wrap_UnknownWrenchContact_contactId_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1509: flag=_wrap_UnknownWrenchContact_contactId_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1510: flag=_wrap_delete_UnknownWrenchContact(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1511: flag=_wrap_new_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1512: flag=_wrap_LinkUnknownWrenchContacts_clear(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1513: flag=_wrap_LinkUnknownWrenchContacts_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1514: flag=_wrap_LinkUnknownWrenchContacts_getNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1515: flag=_wrap_LinkUnknownWrenchContacts_setNrOfContactsForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1516: flag=_wrap_LinkUnknownWrenchContacts_addNewContactForLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1517: flag=_wrap_LinkUnknownWrenchContacts_addNewContactInFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1518: flag=_wrap_LinkUnknownWrenchContacts_addNewUnknownFullWrenchInFrameOrigin(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1519: flag=_wrap_LinkUnknownWrenchContacts_contactWrench(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1520: flag=_wrap_LinkUnknownWrenchContacts_toString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1521: flag=_wrap_delete_LinkUnknownWrenchContacts(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1522: flag=_wrap_new_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1523: flag=_wrap_estimateExternalWrenchesBuffers_resize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1524: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfSubModels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1525: flag=_wrap_estimateExternalWrenchesBuffers_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1526: flag=_wrap_estimateExternalWrenchesBuffers_isConsistent(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1527: flag=_wrap_estimateExternalWrenchesBuffers_A_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1528: flag=_wrap_estimateExternalWrenchesBuffers_A_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1529: flag=_wrap_estimateExternalWrenchesBuffers_x_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1530: flag=_wrap_estimateExternalWrenchesBuffers_x_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1531: flag=_wrap_estimateExternalWrenchesBuffers_b_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1532: flag=_wrap_estimateExternalWrenchesBuffers_b_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1533: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1534: flag=_wrap_estimateExternalWrenchesBuffers_pinvA_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1535: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1536: flag=_wrap_estimateExternalWrenchesBuffers_b_contacts_subtree_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1537: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1538: flag=_wrap_estimateExternalWrenchesBuffers_subModelBase_H_link_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1539: flag=_wrap_delete_estimateExternalWrenchesBuffers(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1540: flag=_wrap_estimateExternalWrenchesWithoutInternalFT(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1541: flag=_wrap_estimateExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1542: flag=_wrap_dynamicsEstimationForwardVelAccKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1543: flag=_wrap_dynamicsEstimationForwardVelKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1544: flag=_wrap_computeLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1545: flag=_wrap_estimateLinkContactWrenchesFromLinkNetExternalWrenches(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1546: flag=_wrap_new_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1547: flag=_wrap_delete_ExtWrenchesAndJointTorquesEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1548: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_setModelAndSensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1549: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1550: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_loadModelAndSensorsFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1551: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1552: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1553: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_submodels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1554: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1555: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1556: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_computeExpectedFTSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1557: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateExtWrenchesAndJointTorques(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1558: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_checkThatTheModelIsStill(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1559: flag=_wrap_ExtWrenchesAndJointTorquesEstimator_estimateLinkNetWrenchesWithoutGravity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1560: flag=_wrap_new_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1561: flag=_wrap_delete_SimpleLeggedOdometry(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1562: flag=_wrap_SimpleLeggedOdometry_setModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1563: flag=_wrap_SimpleLeggedOdometry_loadModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1564: flag=_wrap_SimpleLeggedOdometry_loadModelFromFileWithSpecifiedDOFs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1565: flag=_wrap_SimpleLeggedOdometry_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1566: flag=_wrap_SimpleLeggedOdometry_updateKinematics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1567: flag=_wrap_SimpleLeggedOdometry_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1568: flag=_wrap_SimpleLeggedOdometry_changeFixedFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1569: flag=_wrap_SimpleLeggedOdometry_getCurrentFixedLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1570: flag=_wrap_SimpleLeggedOdometry_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1571: flag=_wrap_SimpleLeggedOdometry_getWorldFrameTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1572: flag=_wrap_isLinkBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1573: flag=_wrap_isJointBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1574: flag=_wrap_isDOFBerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1575: flag=_wrap_new_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1576: flag=_wrap_BerdyOptions_berdyVariant_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1577: flag=_wrap_BerdyOptions_berdyVariant_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1578: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1579: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsDynamicVariables_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1580: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1581: flag=_wrap_BerdyOptions_includeAllJointAccelerationsAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1582: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1583: flag=_wrap_BerdyOptions_includeAllJointTorquesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1584: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1585: flag=_wrap_BerdyOptions_includeAllNetExternalWrenchesAsSensors_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1586: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1587: flag=_wrap_BerdyOptions_includeFixedBaseExternalWrench_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1588: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1589: flag=_wrap_BerdyOptions_jointOnWhichTheInternalWrenchIsMeasured_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1590: flag=_wrap_BerdyOptions_baseLink_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1591: flag=_wrap_BerdyOptions_baseLink_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1592: flag=_wrap_BerdyOptions_checkConsistency(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1593: flag=_wrap_delete_BerdyOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1594: flag=_wrap_BerdySensor_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1595: flag=_wrap_BerdySensor_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1596: flag=_wrap_BerdySensor_id_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1597: flag=_wrap_BerdySensor_id_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1598: flag=_wrap_BerdySensor_range_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1599: flag=_wrap_BerdySensor_range_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1600: flag=_wrap_BerdySensor_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1601: flag=_wrap_BerdySensor_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1602: flag=_wrap_new_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1603: flag=_wrap_delete_BerdySensor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1604: flag=_wrap_BerdyDynamicVariable_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1605: flag=_wrap_BerdyDynamicVariable_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1606: flag=_wrap_BerdyDynamicVariable_id_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1607: flag=_wrap_BerdyDynamicVariable_id_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1608: flag=_wrap_BerdyDynamicVariable_range_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1609: flag=_wrap_BerdyDynamicVariable_range_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1610: flag=_wrap_BerdyDynamicVariable_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1611: flag=_wrap_BerdyDynamicVariable_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1612: flag=_wrap_new_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1613: flag=_wrap_delete_BerdyDynamicVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1614: flag=_wrap_new_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1615: flag=_wrap_BerdyHelper_dynamicTraversal(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1616: flag=_wrap_BerdyHelper_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1617: flag=_wrap_BerdyHelper_sensors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1618: flag=_wrap_BerdyHelper_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1619: flag=_wrap_BerdyHelper_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1620: flag=_wrap_BerdyHelper_getOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1621: flag=_wrap_BerdyHelper_getNrOfDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1622: flag=_wrap_BerdyHelper_getNrOfDynamicEquations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1623: flag=_wrap_BerdyHelper_getNrOfSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1624: flag=_wrap_BerdyHelper_resizeAndZeroBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1625: flag=_wrap_BerdyHelper_getBerdyMatrices(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1626: flag=_wrap_BerdyHelper_getSensorsOrdering(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1627: flag=_wrap_BerdyHelper_getRangeSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1628: flag=_wrap_BerdyHelper_getRangeDOFSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1629: flag=_wrap_BerdyHelper_getRangeJointSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1630: flag=_wrap_BerdyHelper_getRangeLinkSensorVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1631: flag=_wrap_BerdyHelper_getRangeLinkVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1632: flag=_wrap_BerdyHelper_getRangeJointVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1633: flag=_wrap_BerdyHelper_getRangeDOFVariable(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1634: flag=_wrap_BerdyHelper_getDynamicVariablesOrdering(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1635: flag=_wrap_BerdyHelper_serializeDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1636: flag=_wrap_BerdyHelper_serializeSensorVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1637: flag=_wrap_BerdyHelper_serializeDynamicVariablesComputedFromFixedBaseRNEA(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1638: flag=_wrap_BerdyHelper_extractJointTorquesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1639: flag=_wrap_BerdyHelper_extractLinkNetExternalWrenchesFromDynamicVariables(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1640: flag=_wrap_BerdyHelper_updateKinematicsFromFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1641: flag=_wrap_BerdyHelper_updateKinematicsFromFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1642: flag=_wrap_BerdyHelper_updateKinematicsFromTraversalFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1643: flag=_wrap_BerdyHelper_setNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1644: flag=_wrap_BerdyHelper_getNetExternalWrenchMeasurementFrame(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1645: flag=_wrap_delete_BerdyHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1646: flag=_wrap_new_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1647: flag=_wrap_delete_BerdySparseMAPSolver(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1648: flag=_wrap_BerdySparseMAPSolver_setDynamicsConstraintsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1649: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1650: flag=_wrap_BerdySparseMAPSolver_setDynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1651: flag=_wrap_BerdySparseMAPSolver_setMeasurementsPriorCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1652: flag=_wrap_BerdySparseMAPSolver_dynamicsConstraintsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1653: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1654: flag=_wrap_BerdySparseMAPSolver_dynamicsRegularizationPriorExpectedValue(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1655: flag=_wrap_BerdySparseMAPSolver_measurementsPriorCovarianceInverse(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1656: flag=_wrap_BerdySparseMAPSolver_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1657: flag=_wrap_BerdySparseMAPSolver_initialize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1658: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFixedBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1659: flag=_wrap_BerdySparseMAPSolver_updateEstimateInformationFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1660: flag=_wrap_BerdySparseMAPSolver_doEstimate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1661: flag=_wrap_BerdySparseMAPSolver_getLastEstimate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1662: flag=_wrap_AttitudeEstimatorState_m_orientation_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1663: flag=_wrap_AttitudeEstimatorState_m_orientation_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1664: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1665: flag=_wrap_AttitudeEstimatorState_m_angular_velocity_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1666: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1667: flag=_wrap_AttitudeEstimatorState_m_gyroscope_bias_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1668: flag=_wrap_new_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1669: flag=_wrap_delete_AttitudeEstimatorState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1670: flag=_wrap_delete_IAttitudeEstimator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1671: flag=_wrap_IAttitudeEstimator_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1672: flag=_wrap_IAttitudeEstimator_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1673: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1674: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1675: flag=_wrap_IAttitudeEstimator_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1676: flag=_wrap_IAttitudeEstimator_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1677: flag=_wrap_IAttitudeEstimator_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1678: flag=_wrap_IAttitudeEstimator_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1679: flag=_wrap_IAttitudeEstimator_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1680: flag=_wrap_IAttitudeEstimator_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1681: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1682: flag=_wrap_AttitudeMahonyFilterParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1683: flag=_wrap_AttitudeMahonyFilterParameters_kp_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1684: flag=_wrap_AttitudeMahonyFilterParameters_kp_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1685: flag=_wrap_AttitudeMahonyFilterParameters_ki_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1686: flag=_wrap_AttitudeMahonyFilterParameters_ki_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1687: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1688: flag=_wrap_AttitudeMahonyFilterParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1689: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1690: flag=_wrap_AttitudeMahonyFilterParameters_confidence_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1691: flag=_wrap_new_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1692: flag=_wrap_delete_AttitudeMahonyFilterParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1693: flag=_wrap_new_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1694: flag=_wrap_AttitudeMahonyFilter_useMagnetoMeterMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1695: flag=_wrap_AttitudeMahonyFilter_setConfidenceForMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1696: flag=_wrap_AttitudeMahonyFilter_setGainkp(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1697: flag=_wrap_AttitudeMahonyFilter_setGainki(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1698: flag=_wrap_AttitudeMahonyFilter_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1699: flag=_wrap_AttitudeMahonyFilter_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1700: flag=_wrap_AttitudeMahonyFilter_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1701: flag=_wrap_AttitudeMahonyFilter_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1702: flag=_wrap_AttitudeMahonyFilter_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1703: flag=_wrap_AttitudeMahonyFilter_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1704: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1705: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1706: flag=_wrap_AttitudeMahonyFilter_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1707: flag=_wrap_AttitudeMahonyFilter_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1708: flag=_wrap_AttitudeMahonyFilter_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1709: flag=_wrap_AttitudeMahonyFilter_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1710: flag=_wrap_AttitudeMahonyFilter_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1711: flag=_wrap_AttitudeMahonyFilter_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1712: flag=_wrap_delete_AttitudeMahonyFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1713: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_f(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1714: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekf_h(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1715: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1716: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfComputeJacobianH(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1717: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfPredict(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1718: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfUpdate(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1719: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfInit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1720: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfReset(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1721: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1722: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1723: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1724: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1725: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetSystemNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1726: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetMeasurementNoiseCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1727: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1728: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetInputSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1729: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfSetOutputSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1730: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1731: flag=_wrap_DiscreteExtendedKalmanFilterHelper_ekfGetStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1732: flag=_wrap_delete_DiscreteExtendedKalmanFilterHelper(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1733: flag=_wrap_output_dimensions_with_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1734: flag=_wrap_output_dimensions_without_magnetometer_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1735: flag=_wrap_input_dimensions_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1736: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1737: flag=_wrap_AttitudeQuaternionEKFParameters_time_step_in_seconds_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1738: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1739: flag=_wrap_AttitudeQuaternionEKFParameters_bias_correlation_time_factor_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1740: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1741: flag=_wrap_AttitudeQuaternionEKFParameters_accelerometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1742: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1743: flag=_wrap_AttitudeQuaternionEKFParameters_magnetometer_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1744: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1745: flag=_wrap_AttitudeQuaternionEKFParameters_gyroscope_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1746: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1747: flag=_wrap_AttitudeQuaternionEKFParameters_gyro_bias_noise_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1748: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1749: flag=_wrap_AttitudeQuaternionEKFParameters_initial_orientation_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1750: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1751: flag=_wrap_AttitudeQuaternionEKFParameters_initial_ang_vel_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1752: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1753: flag=_wrap_AttitudeQuaternionEKFParameters_initial_gyro_bias_error_variance_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1754: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1755: flag=_wrap_AttitudeQuaternionEKFParameters_use_magnetometer_measurements_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1756: flag=_wrap_new_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1757: flag=_wrap_delete_AttitudeQuaternionEKFParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1758: flag=_wrap_new_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1759: flag=_wrap_AttitudeQuaternionEKF_getParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1760: flag=_wrap_AttitudeQuaternionEKF_setParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1761: flag=_wrap_AttitudeQuaternionEKF_setGravityDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1762: flag=_wrap_AttitudeQuaternionEKF_setTimeStepInSeconds(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1763: flag=_wrap_AttitudeQuaternionEKF_setBiasCorrelationTimeFactor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1764: flag=_wrap_AttitudeQuaternionEKF_useMagnetometerMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1765: flag=_wrap_AttitudeQuaternionEKF_setMeasurementNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1766: flag=_wrap_AttitudeQuaternionEKF_setSystemNoiseVariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1767: flag=_wrap_AttitudeQuaternionEKF_setInitialStateCovariance(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1768: flag=_wrap_AttitudeQuaternionEKF_initializeFilter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1769: flag=_wrap_AttitudeQuaternionEKF_updateFilterWithMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1770: flag=_wrap_AttitudeQuaternionEKF_propagateStates(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1771: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRotationMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1772: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsQuaternion(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1773: flag=_wrap_AttitudeQuaternionEKF_getOrientationEstimateAsRPY(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1774: flag=_wrap_AttitudeQuaternionEKF_getInternalStateSize(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1775: flag=_wrap_AttitudeQuaternionEKF_getInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1776: flag=_wrap_AttitudeQuaternionEKF_getDefaultInternalInitialState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1777: flag=_wrap_AttitudeQuaternionEKF_setInternalState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1778: flag=_wrap_AttitudeQuaternionEKF_setInternalStateInitialOrientation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1779: flag=_wrap_delete_AttitudeQuaternionEKF(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1780: flag=_wrap_estimateInertialParametersFromLinkBoundingBoxesAndTotalMass(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1781: flag=_wrap_DynamicsRegressorParameter_category_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1782: flag=_wrap_DynamicsRegressorParameter_category_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1783: flag=_wrap_DynamicsRegressorParameter_elemIndex_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1784: flag=_wrap_DynamicsRegressorParameter_elemIndex_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1785: flag=_wrap_DynamicsRegressorParameter_type_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1786: flag=_wrap_DynamicsRegressorParameter_type_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1787: flag=_wrap_DynamicsRegressorParameter_lt(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1788: flag=_wrap_DynamicsRegressorParameter_eq(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1789: flag=_wrap_DynamicsRegressorParameter_ne(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1790: flag=_wrap_new_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1791: flag=_wrap_delete_DynamicsRegressorParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1792: flag=_wrap_DynamicsRegressorParametersList_parameters_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1793: flag=_wrap_DynamicsRegressorParametersList_parameters_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1794: flag=_wrap_DynamicsRegressorParametersList_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1795: flag=_wrap_DynamicsRegressorParametersList_addParam(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1796: flag=_wrap_DynamicsRegressorParametersList_addList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1797: flag=_wrap_DynamicsRegressorParametersList_findParam(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1798: flag=_wrap_DynamicsRegressorParametersList_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1799: flag=_wrap_new_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1800: flag=_wrap_delete_DynamicsRegressorParametersList(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1801: flag=_wrap_new_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1802: flag=_wrap_delete_DynamicsRegressorGenerator(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1803: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1804: flag=_wrap_DynamicsRegressorGenerator_loadRobotAndSensorsModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1805: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1806: flag=_wrap_DynamicsRegressorGenerator_loadRegressorStructureFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1807: flag=_wrap_DynamicsRegressorGenerator_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1808: flag=_wrap_DynamicsRegressorGenerator_getNrOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1809: flag=_wrap_DynamicsRegressorGenerator_getNrOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1810: flag=_wrap_DynamicsRegressorGenerator_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1811: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameter(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1812: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1813: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutput(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1814: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfOutputs(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1815: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1816: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1817: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLink(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1818: flag=_wrap_DynamicsRegressorGenerator_getDescriptionOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1819: flag=_wrap_DynamicsRegressorGenerator_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1820: flag=_wrap_DynamicsRegressorGenerator_getNrOfFakeLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1821: flag=_wrap_DynamicsRegressorGenerator_getBaseLinkName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1822: flag=_wrap_DynamicsRegressorGenerator_getSensorsModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1823: flag=_wrap_DynamicsRegressorGenerator_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1824: flag=_wrap_DynamicsRegressorGenerator_getSensorsMeasurements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1825: flag=_wrap_DynamicsRegressorGenerator_setTorqueSensorMeasurement(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1826: flag=_wrap_DynamicsRegressorGenerator_computeRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1827: flag=_wrap_DynamicsRegressorGenerator_getModelParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1828: flag=_wrap_DynamicsRegressorGenerator_computeFloatingBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1829: flag=_wrap_DynamicsRegressorGenerator_computeFixedBaseIdentifiableSubspace(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1830: flag=_wrap_DynamicsRegressorGenerator_generate_random_regressors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1831: flag=_wrap_new_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1832: flag=_wrap_delete_KinDynComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1833: flag=_wrap_KinDynComputations_loadRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1834: flag=_wrap_KinDynComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1835: flag=_wrap_KinDynComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1836: flag=_wrap_KinDynComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1837: flag=_wrap_KinDynComputations_setFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1838: flag=_wrap_KinDynComputations_getFrameVelocityRepresentation(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1839: flag=_wrap_KinDynComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1840: flag=_wrap_KinDynComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1841: flag=_wrap_KinDynComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1842: flag=_wrap_KinDynComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1843: flag=_wrap_KinDynComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1844: flag=_wrap_KinDynComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1845: flag=_wrap_KinDynComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1846: flag=_wrap_KinDynComputations_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1847: flag=_wrap_KinDynComputations_getRobotModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1848: flag=_wrap_KinDynComputations_getRelativeJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1849: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobianSparsityPattern(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1850: flag=_wrap_KinDynComputations_setJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1851: flag=_wrap_KinDynComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1852: flag=_wrap_KinDynComputations_getRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1853: flag=_wrap_KinDynComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1854: flag=_wrap_KinDynComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1855: flag=_wrap_KinDynComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1856: flag=_wrap_KinDynComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1857: flag=_wrap_KinDynComputations_getModelVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1858: flag=_wrap_KinDynComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1859: flag=_wrap_KinDynComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1860: flag=_wrap_KinDynComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1861: flag=_wrap_KinDynComputations_getRelativeTransformExplicit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1862: flag=_wrap_KinDynComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1863: flag=_wrap_KinDynComputations_getFrameVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1864: flag=_wrap_KinDynComputations_getFrameAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1865: flag=_wrap_KinDynComputations_getFrameFreeFloatingJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1866: flag=_wrap_KinDynComputations_getRelativeJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1867: flag=_wrap_KinDynComputations_getRelativeJacobianExplicit(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1868: flag=_wrap_KinDynComputations_getFrameBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1869: flag=_wrap_KinDynComputations_getCenterOfMassPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1870: flag=_wrap_KinDynComputations_getCenterOfMassVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1871: flag=_wrap_KinDynComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1872: flag=_wrap_KinDynComputations_getCenterOfMassBiasAcc(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1873: flag=_wrap_KinDynComputations_getAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1874: flag=_wrap_KinDynComputations_getAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1875: flag=_wrap_KinDynComputations_getCentroidalAverageVelocity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1876: flag=_wrap_KinDynComputations_getCentroidalAverageVelocityJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1877: flag=_wrap_KinDynComputations_getLinearAngularMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1878: flag=_wrap_KinDynComputations_getLinearAngularMomentumJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1879: flag=_wrap_KinDynComputations_getCentroidalTotalMomentum(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1880: flag=_wrap_KinDynComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1881: flag=_wrap_KinDynComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1882: flag=_wrap_KinDynComputations_generalizedBiasForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1883: flag=_wrap_KinDynComputations_generalizedGravityForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1884: flag=_wrap_KinDynComputations_generalizedExternalForces(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1885: flag=_wrap_KinDynComputations_inverseDynamicsInertialParametersRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1886: flag=_wrap_delete_ICamera(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1887: flag=_wrap_ICamera_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1888: flag=_wrap_ICamera_setTarget(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1889: flag=_wrap_ICamera_setUpVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1890: flag=_wrap_ColorViz_r_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1891: flag=_wrap_ColorViz_r_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1892: flag=_wrap_ColorViz_g_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1893: flag=_wrap_ColorViz_g_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1894: flag=_wrap_ColorViz_b_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1895: flag=_wrap_ColorViz_b_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1896: flag=_wrap_ColorViz_a_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1897: flag=_wrap_ColorViz_a_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1898: flag=_wrap_new_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1899: flag=_wrap_delete_ColorViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1900: flag=_wrap_delete_ILight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1901: flag=_wrap_ILight_getName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1902: flag=_wrap_ILight_setType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1903: flag=_wrap_ILight_getType(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1904: flag=_wrap_ILight_setPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1905: flag=_wrap_ILight_getPosition(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1906: flag=_wrap_ILight_setDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1907: flag=_wrap_ILight_getDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1908: flag=_wrap_ILight_setAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1909: flag=_wrap_ILight_getAmbientColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1910: flag=_wrap_ILight_setSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1911: flag=_wrap_ILight_getSpecularColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1912: flag=_wrap_ILight_setDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1913: flag=_wrap_ILight_getDiffuseColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1914: flag=_wrap_delete_IEnvironment(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1915: flag=_wrap_IEnvironment_getElements(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1916: flag=_wrap_IEnvironment_setElementVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1917: flag=_wrap_IEnvironment_setBackgroundColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1918: flag=_wrap_IEnvironment_setAmbientLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1919: flag=_wrap_IEnvironment_getLights(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1920: flag=_wrap_IEnvironment_addLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1921: flag=_wrap_IEnvironment_lightViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1922: flag=_wrap_IEnvironment_removeLight(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1923: flag=_wrap_delete_IJetsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1924: flag=_wrap_IJetsVisualization_setJetsFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1925: flag=_wrap_IJetsVisualization_getNrOfJets(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1926: flag=_wrap_IJetsVisualization_getJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1927: flag=_wrap_IJetsVisualization_setJetDirection(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1928: flag=_wrap_IJetsVisualization_setJetColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1929: flag=_wrap_IJetsVisualization_setJetsDimensions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1930: flag=_wrap_IJetsVisualization_setJetsIntensity(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1931: flag=_wrap_delete_IVectorsVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1932: flag=_wrap_IVectorsVisualization_addVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1933: flag=_wrap_IVectorsVisualization_getNrOfVectors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1934: flag=_wrap_IVectorsVisualization_getVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1935: flag=_wrap_IVectorsVisualization_updateVector(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1936: flag=_wrap_IVectorsVisualization_setVectorColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1937: flag=_wrap_IVectorsVisualization_setVectorsAspect(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1938: flag=_wrap_delete_IModelVisualization(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1939: flag=_wrap_IModelVisualization_setPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1940: flag=_wrap_IModelVisualization_setLinkPositions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1941: flag=_wrap_IModelVisualization_model(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1942: flag=_wrap_IModelVisualization_getInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1943: flag=_wrap_IModelVisualization_setModelVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1944: flag=_wrap_IModelVisualization_setModelColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1945: flag=_wrap_IModelVisualization_resetModelColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1946: flag=_wrap_IModelVisualization_setLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1947: flag=_wrap_IModelVisualization_resetLinkColor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1948: flag=_wrap_IModelVisualization_getLinkNames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1949: flag=_wrap_IModelVisualization_setLinkVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1950: flag=_wrap_IModelVisualization_getFeatures(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1951: flag=_wrap_IModelVisualization_setFeatureVisibility(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1952: flag=_wrap_IModelVisualization_jets(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1953: flag=_wrap_IModelVisualization_getWorldModelTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1954: flag=_wrap_IModelVisualization_getWorldLinkTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1955: flag=_wrap_VisualizerOptions_verbose_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1956: flag=_wrap_VisualizerOptions_verbose_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1957: flag=_wrap_VisualizerOptions_winWidth_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1958: flag=_wrap_VisualizerOptions_winWidth_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1959: flag=_wrap_VisualizerOptions_winHeight_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1960: flag=_wrap_VisualizerOptions_winHeight_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1961: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_get(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1962: flag=_wrap_VisualizerOptions_rootFrameArrowsDimension_set(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1963: flag=_wrap_new_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1964: flag=_wrap_delete_VisualizerOptions(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1965: flag=_wrap_new_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1966: flag=_wrap_delete_Visualizer(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1967: flag=_wrap_Visualizer_init(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1968: flag=_wrap_Visualizer_getNrOfVisualizedModels(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1969: flag=_wrap_Visualizer_getModelInstanceName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1970: flag=_wrap_Visualizer_getModelInstanceIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1971: flag=_wrap_Visualizer_addModel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1972: flag=_wrap_Visualizer_modelViz(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1973: flag=_wrap_Visualizer_camera(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1974: flag=_wrap_Visualizer_enviroment(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1975: flag=_wrap_Visualizer_vectors(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1976: flag=_wrap_Visualizer_run(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1977: flag=_wrap_Visualizer_draw(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1978: flag=_wrap_Visualizer_drawToFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1979: flag=_wrap_Visualizer_close(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1980: flag=_wrap_new_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1981: flag=_wrap_delete_DynamicsComputations(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1982: flag=_wrap_DynamicsComputations_loadRobotModelFromFile(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1983: flag=_wrap_DynamicsComputations_loadRobotModelFromString(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1984: flag=_wrap_DynamicsComputations_isValid(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1985: flag=_wrap_DynamicsComputations_getNrOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1986: flag=_wrap_DynamicsComputations_getDescriptionOfDegreeOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1987: flag=_wrap_DynamicsComputations_getDescriptionOfDegreesOfFreedom(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1988: flag=_wrap_DynamicsComputations_getNrOfLinks(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1989: flag=_wrap_DynamicsComputations_getNrOfFrames(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1990: flag=_wrap_DynamicsComputations_getFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1991: flag=_wrap_DynamicsComputations_setFloatingBase(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1992: flag=_wrap_DynamicsComputations_setRobotState(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1993: flag=_wrap_DynamicsComputations_getWorldBaseTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1994: flag=_wrap_DynamicsComputations_getBaseTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1995: flag=_wrap_DynamicsComputations_getJointPos(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1996: flag=_wrap_DynamicsComputations_getJointVel(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1997: flag=_wrap_DynamicsComputations_getFrameIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1998: flag=_wrap_DynamicsComputations_getFrameName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 1999: flag=_wrap_DynamicsComputations_getWorldTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2000: flag=_wrap_DynamicsComputations_getRelativeTransform(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2001: flag=_wrap_DynamicsComputations_getFrameTwist(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2002: flag=_wrap_DynamicsComputations_getFrameTwistInWorldOrient(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2003: flag=_wrap_DynamicsComputations_getFrameProperSpatialAcceleration(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2004: flag=_wrap_DynamicsComputations_getLinkIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2005: flag=_wrap_DynamicsComputations_getLinkInertia(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2006: flag=_wrap_DynamicsComputations_getJointIndex(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2007: flag=_wrap_DynamicsComputations_getJointName(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2008: flag=_wrap_DynamicsComputations_getJointLimits(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2009: flag=_wrap_DynamicsComputations_inverseDynamics(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2010: flag=_wrap_DynamicsComputations_getFreeFloatingMassMatrix(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2011: flag=_wrap_DynamicsComputations_getFrameJacobian(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2012: flag=_wrap_DynamicsComputations_getDynamicsRegressor(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2013: flag=_wrap_DynamicsComputations_getModelDynamicsParameters(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2014: flag=_wrap_DynamicsComputations_getCenterOfMass(resc,resv,argc,(mxArray**)(argv)); break;
+  case 2015: flag=_wrap_DynamicsComputations_getCenterOfMassJacobian(resc,resv,argc,(mxArray**)(argv)); break;
   default: flag=1, SWIG_Error(SWIG_RuntimeError, "No function id %d.", fcn_id);
   }
   if (flag) {

From bea3a74062e9ba37ec5fe23805b599191c52ebb5 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 3 Oct 2019 14:52:55 +0200
Subject: [PATCH 155/194] Bump patch version

---
 CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e21d1f53750..691ef72e7eb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,7 +25,7 @@ set(VARS_PREFIX "iDynTree")
 
 set(${VARS_PREFIX}_MAJOR_VERSION 0)
 set(${VARS_PREFIX}_MINOR_VERSION 11)
-set(${VARS_PREFIX}_PATCH_VERSION 104)
+set(${VARS_PREFIX}_PATCH_VERSION 105)
 set(${VARS_PREFIX}_VERSION ${${VARS_PREFIX}_MAJOR_VERSION}.${${VARS_PREFIX}_MINOR_VERSION}.${${VARS_PREFIX}_PATCH_VERSION})
 
 # Pick up our CMake scripts - they are all in the cmake subdirectory.

From db0eefb9f41f7a2ece8abd046dda5be3aee52c53 Mon Sep 17 00:00:00 2001
From: Lorenzo Rapetti <lor.rapetti@gmail.com>
Date: Thu, 3 Oct 2019 15:30:42 +0200
Subject: [PATCH 156/194] Update v0_12.md

---
 doc/releases/v0_12.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 66cecb4ec86..8ef934fcc51 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -54,6 +54,8 @@ to normalize errors or as initial points of a nonlinear optimization procedure.
 * Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
 * Added `DiscreteKalmanFilterHelper` class for an implementation of a discrete, linear time-invariant Kalman Filter  (https://github.com/robotology/idyntree/pull/559)
 * Added dynamic reset functionality to `DiscreteExtendedKalmanFilterHelper` class (https://github.com/robotology/idyntree/pull/553)
+* Fixed missing `DOF_ACCELLERATION` data in dynamic variable cache ordering
+(https://github.com/robotology/idyntree/pull/587)
 
 #### `bindings`
 * Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)

From 0ecf32653c95e74fa25dfc235a7d7f811476de7e Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 7 Oct 2019 22:18:15 +0200
Subject: [PATCH 157/194] Fix CMake warning in solid-shapes if

---
 src/solid-shapes/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/solid-shapes/CMakeLists.txt b/src/solid-shapes/CMakeLists.txt
index 62453096344..73d80c0b51f 100644
--- a/src/solid-shapes/CMakeLists.txt
+++ b/src/solid-shapes/CMakeLists.txt
@@ -36,4 +36,4 @@ set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_S
 
 if(IDYNTREE_COMPILE_TESTS AND IDYNTREE_USES_ASSIMP)
     add_subdirectory(tests)
-endif(IDYNTREE_COMPILE_TESTS)
+endif()

From c38661c180262e9e622397fd9539d899546f45bd Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 7 Oct 2019 22:32:01 +0200
Subject: [PATCH 158/194] Do not search for mscgen

The mscgen check was added to use the mscgen tool, then was never actually used, see https://github.com/robotology/idyntree/issues/114 .
---
 doc/CMakeLists.txt | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt
index ba340a31ae3..78ad3ec4554 100644
--- a/doc/CMakeLists.txt
+++ b/doc/CMakeLists.txt
@@ -34,19 +34,6 @@ if(DOXYGEN_FOUND)
     set(DOX_PATTERNS "*.h *.dox *.cpp *.hpp")
     set(DOX_GENERATE_MAN NO)
 
-    # search mscgen program
-    # for setting the MSCGEN_PATH variable in the Doxyfile
-    find_program(MSCGEN_EXECUTABLE mscgen)
-
-    if( MSCGEN_EXECUTABLE STREQUAL "MSCGEN_EXECUTABLE-NOTFOUND" )
-        message(STATUS "mscgen program not found")
-        set(MSCGEN_PATH "")
-    else()
-        # set the MSCGEN_PATH variable by stripping the name (mscgen)
-        # from the MSCGEN_EXECUTABLE
-        string(REGEX REPLACE "mscgen$" "" MSCGEN_PATH ${MSCGEN_EXECUTABLE})
-    endif()
-
     # search dot program
     # for setting the DOT_PATH variable in the Doxyfile
     find_program(DOT_EXECUTABLE dot)

From 36930b4df5dd1aef7e8b942087863dfe8ca8bb98 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sun, 10 Nov 2019 19:47:16 +0100
Subject: [PATCH 159/194] add operator[] to VectorDynSize

---
 src/core/include/iDynTree/Core/VectorDynSize.h |  4 ++++
 src/core/src/VectorDynSize.cpp                 | 12 ++++++++++++
 2 files changed, 16 insertions(+)

diff --git a/src/core/include/iDynTree/Core/VectorDynSize.h b/src/core/include/iDynTree/Core/VectorDynSize.h
index 8a302911ef3..e418efe4ed5 100644
--- a/src/core/include/iDynTree/Core/VectorDynSize.h
+++ b/src/core/include/iDynTree/Core/VectorDynSize.h
@@ -119,6 +119,10 @@ namespace iDynTree
 
         double& operator()(const unsigned int index);
 
+        double operator[](const unsigned int index) const;
+
+        double& operator[](const unsigned int index);
+
         double getVal(const unsigned int index) const;
 
         bool setVal(const unsigned int index, const double new_el);
diff --git a/src/core/src/VectorDynSize.cpp b/src/core/src/VectorDynSize.cpp
index 2587a4806c8..30aefeb7f9b 100644
--- a/src/core/src/VectorDynSize.cpp
+++ b/src/core/src/VectorDynSize.cpp
@@ -153,6 +153,18 @@ double VectorDynSize::operator()(unsigned int index) const
     return this->m_data[index];
 }
 
+double& VectorDynSize::operator[](unsigned int index)
+{
+    assert(index < this->size());
+    return this->m_data[index];
+}
+
+double VectorDynSize::operator[](unsigned int index) const
+{
+    assert(index < this->size());
+    return this->m_data[index];
+}
+
 double VectorDynSize::getVal(const unsigned int index) const
 {
     if( index >= this->size() )

From a535eb89e28bfcb2a8e031ff74b6cf070879001b Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sun, 10 Nov 2019 19:48:30 +0100
Subject: [PATCH 160/194] add operator[] to VectorFixSize

---
 .../include/iDynTree/Core/VectorFixSize.h     | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/src/core/include/iDynTree/Core/VectorFixSize.h b/src/core/include/iDynTree/Core/VectorFixSize.h
index 9bb3c3e10b0..504f6b0f3fd 100644
--- a/src/core/include/iDynTree/Core/VectorFixSize.h
+++ b/src/core/include/iDynTree/Core/VectorFixSize.h
@@ -63,6 +63,10 @@ namespace iDynTree
 
         double& operator()(const unsigned int index);
 
+        double operator[](const unsigned int index) const;
+
+        double& operator[](const unsigned int index);
+
         double getVal(const unsigned int index) const;
 
         bool setVal(const unsigned int index, const double new_el);
@@ -144,7 +148,7 @@ namespace iDynTree
     template<unsigned int VecSize>
     VectorFixSize<VecSize>::VectorFixSize()
     {
-        
+
     }
 
 
@@ -214,6 +218,19 @@ namespace iDynTree
         return this->m_data[index];
     }
 
+    template<unsigned int VecSize>
+    double VectorFixSize<VecSize>::operator[](const unsigned int index) const
+    {
+        assert(index < VecSize);
+        return this->m_data[index];
+    }
+
+    template<unsigned int VecSize>
+    double & VectorFixSize<VecSize>::operator[](const unsigned int index)
+    {
+        assert(index < VecSize);
+        return this->m_data[index];
+    }
 
     template<unsigned int VecSize>
     double VectorFixSize<VecSize>::getVal(const unsigned int index) const

From 4ea19e001eec09b9179e2b644068cc11a6f822cf Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Sun, 10 Nov 2019 19:50:51 +0100
Subject: [PATCH 161/194] update changelog

---
 doc/releases/v0_12.md | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index 66cecb4ec86..870bec71d4a 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -67,6 +67,8 @@ to normalize errors or as initial points of a nonlinear optimization procedure.
 * Fixed implementation of Transform::log() method (https://github.com/robotology/idyntree/pull/562)
 * Fixed implementation of SpatialMotionVector::exp() method (https://github.com/robotology/idyntree/pull/562)
 * Add nameIsValid attribute to iDynTree::SolidShape class.
+* Add operator[] method to `iDynTree::VectorDynSize` (https://github.com/robotology/idyntree/pull/596)
+* Add operator[] method to `iDynTree::VectorFixSize` (https://github.com/robotology/idyntree/pull/596)
 
 ### `Model`
 * Implement `getTotalMass()` method for Model class

From 36307d60bea3c879864eaddb1fd831fec1adc444 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 26 Nov 2019 13:15:01 +0100
Subject: [PATCH 162/194] Do not quietly search for ASSIMP to decide if enable
 it or not (#600)

Add DO_NOT_SILENTLY_SEARCH option to avoid the initial find_package QUIET call to decide the initial default value of the option to enable a dependency.

Enable it for Assimp as a workaround for #599 .

Assimp was added to GitHub Actons CI to validate that the solution is working.
---
 .github/workflows/ci.yml         | 14 ++++++++------
 cmake/iDynTreeDependencies.cmake | 22 ++++++++++++++++------
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 4067ac0a448..a98aab1c807 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -34,12 +34,12 @@ jobs:
       if: matrix.os == 'windows-latest'
       run: |
         git clone https://github.com/robotology-dependencies/robotology-vcpkg-binary-ports C:/robotology-vcpkg-binary-ports
-        vcpkg.exe --overlay-ports=C:/robotology-vcpkg-binary-ports install --triplet x64-windows ace libxml2 eigen3 ipopt-binary 
+        vcpkg.exe --overlay-ports=C:/robotology-vcpkg-binary-ports install --triplet x64-windows ace assimp libxml2 eigen3 ipopt-binary 
         
     - name: Dependencies [macOS]
       if: matrix.os == 'macOS-latest'
       run: |
-        brew install ace boost eigen swig qt5 orocos-kdl
+        brew install ace assimp boost eigen swig qt5 orocos-kdl
     
     - name: Dependencies [Ubuntu]
       if: matrix.os == 'ubuntu-latest'
@@ -47,7 +47,7 @@ jobs:
         sudo apt-get update
         sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev  libboost-system-dev libboost-filesystem-dev \
                              libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \
-                             libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind 
+                             libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind libassimp-dev
         
     - name: Source-based Dependencies [Windows] 
       if: matrix.os == 'windows-latest-disable-until-issue-569-is-solved'
@@ -117,9 +117,10 @@ jobs:
         mkdir -p build
         cd build
         # ipopt disabled until https://github.com/robotology/idyntree/issues/274 is fixed
+        # assimp disabled until https://github.com/robotology/idyntree/issues/601 is fixed
         cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install \
               -DIDYNTREE_COMPILE_TESTS:BOOL=ON -DIDYNTREE_USES_YARP:BOOL=OFF -DIDYNTREE_USES_ICUB_MAIN:BOOL=OFF -DIDYNTREE_RUN_VALGRIND_TESTS:BOOL=ON \
-              -DIDYNTREE_USES_PYTHON:BOOL=OFF -DIDYNTREE_USES_OCTAVE:BOOL=OFF -DIDYNTREE_USES_IPOPT:BOOL=OFF \
+              -DIDYNTREE_USES_PYTHON:BOOL=OFF -DIDYNTREE_USES_OCTAVE:BOOL=OFF -DIDYNTREE_USES_IPOPT:BOOL=OFF -DIDYNTREE_USES_ASSIMP:BOOL=OFF \
               -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
 
     - name: Configure [Ubuntu/macOS]
@@ -127,9 +128,10 @@ jobs:
       shell: bash
       run: |
         mkdir -p build
-        cd build    
+        cd build
+        # IDYNTREE_USES_ASSIMP set to OFF as a workaround for https://github.com/robotology/idyntree/issues/599
         cmake -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install -DIDYNTREE_COMPILE_TESTS:BOOL=ON -DIDYNTREE_USES_YARP:BOOL=ON \
-              -DIDYNTREE_USES_ICUB_MAIN:BOOL=ON -DIDYNTREE_USES_Qt5:BOOL=ON  -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \
+              -DIDYNTREE_USES_ICUB_MAIN:BOOL=ON -DIDYNTREE_USES_Qt5:BOOL=ON  -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} -DIDYNTREE_USES_ASSIMP:BOOL=OFF \
               -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install ..
 
     - name: Enable additional Ubuntu options (Valgrind, Octave, Python, legacy KDL) [Ubuntu] 
diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index e8d46eb89ec..33e110f2d52 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -4,17 +4,26 @@
 
 #########################################################################
 # Enable/disable dependencies
+# DO_NOT_SILENTLY_SEARCH: Do not search for the package to set the default
+#                         value of IDYNTREE_USES_<dep> option, but just set
+#                         it to OFF 
 macro(idyntree_handle_dependency package)
+  set(options DO_NOT_SILENTLY_SEARCH)
   set(singleValueArgs MINIMUM_VERSION)
   set(multiValueArgs COMPONENTS)
   cmake_parse_arguments(IHD "" "${singleValueArgs}" "${multiValueArgs}" ${ARGN})
-  if (IHD_COMPONENTS)
-    find_package(${package} ${IHD_MINIMUM_VERSION} QUIET COMPONENTS ${IHD_COMPONENTS})
+  string(TOUPPER ${package} PKG)
+  if (IHD_DO_NOT_SILENTLY_SEARCH)
+    if (IHD_COMPONENTS)
+      find_package(${package} ${IHD_MINIMUM_VERSION} QUIET COMPONENTS ${IHD_COMPONENTS})
+    else ()
+      find_package(${package} ${IHD_MINIMUM_VERSION} QUIET)
+    endif ()
+    set(IDYNTREE_USES_${PKG}_DEFAULT ${${package}_FOUND})
   else ()
-    find_package(${package} ${IHD_MINIMUM_VERSION} QUIET)
+    set(IDYNTREE_USES_${PKG}_DEFAULT FALSE)
   endif ()
-  string(TOUPPER ${package} PKG)
-  option(IDYNTREE_USES_${PKG} "Build the part of iDynTree that depends on package ${package}" ${${package}_FOUND})
+  option(IDYNTREE_USES_${PKG} "Build the part of iDynTree that depends on package ${package}" ${IDYNTREE_USES_${PKG}_DEFAULT})
   if (IDYNTREE_USES_${PKG})
     if (IHD_COMPONENTS)
       find_package(${package} ${IHD_MINIMUM_VERSION} COMPONENTS ${IHD_COMPONENTS} REQUIRED)
@@ -57,4 +66,5 @@ idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
 idyntree_handle_dependency(ALGLIB 3.14.0)
 idyntree_handle_dependency(WORHP)
-idyntree_handle_dependency(ASSIMP)
+# Workaround for https://github.com/robotology/idyntree/issues/599
+idyntree_handle_dependency(ASSIMP DO_NOT_SILENTLY_SEARCH)

From 0c0a4519b99630e3b17f807204bbb68192955252 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 26 Nov 2019 14:47:22 +0100
Subject: [PATCH 163/194] Fix automatic detection of available dependencies
 (#602)

* Fix automatic detection of available dependencies
---
 cmake/iDynTreeDependencies.cmake | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 33e110f2d52..91093a6e487 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -11,9 +11,9 @@ macro(idyntree_handle_dependency package)
   set(options DO_NOT_SILENTLY_SEARCH)
   set(singleValueArgs MINIMUM_VERSION)
   set(multiValueArgs COMPONENTS)
-  cmake_parse_arguments(IHD "" "${singleValueArgs}" "${multiValueArgs}" ${ARGN})
+  cmake_parse_arguments(IHD "${options}" "${singleValueArgs}" "${multiValueArgs}" ${ARGN})
   string(TOUPPER ${package} PKG)
-  if (IHD_DO_NOT_SILENTLY_SEARCH)
+  if (NOT IHD_DO_NOT_SILENTLY_SEARCH)
     if (IHD_COMPONENTS)
       find_package(${package} ${IHD_MINIMUM_VERSION} QUIET COMPONENTS ${IHD_COMPONENTS})
     else ()

From 5d7de5be6f342939f1b5f7f80850da4bf9c3ff48 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 19 Dec 2019 22:21:56 +0100
Subject: [PATCH 164/194] Add ModelTestUtils.h file to the installed headers

---
 src/model/CMakeLists.txt | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/model/CMakeLists.txt b/src/model/CMakeLists.txt
index f716378e2ab..c4701b49b0f 100644
--- a/src/model/CMakeLists.txt
+++ b/src/model/CMakeLists.txt
@@ -32,9 +32,8 @@ set(IDYNTREE_MODEL_HEADERS include/iDynTree/Model/ContactWrench.h
                            include/iDynTree/Model/PrismaticJoint.h
                            include/iDynTree/Model/SolidShapes.h
                            include/iDynTree/Model/SubModel.h
-                           include/iDynTree/Model/Traversal.h)
-
-set(IDYNTREE_MODEL_PRIVATE_INCLUDES include/iDynTree/Model/ModelTestUtils.h)
+                           include/iDynTree/Model/Traversal.h
+                           include/iDynTree/Model/ModelTestUtils.h)
 
 set(IDYNTREE_MODEL_SOURCES src/ContactWrench.cpp
                            src/DenavitHartenberg.cpp
@@ -62,11 +61,10 @@ set(IDYNTREE_MODEL_SOURCES src/ContactWrench.cpp
 
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_MODEL_SOURCES})
 SOURCE_GROUP("Header Files" FILES ${IDYNTREE_MODEL_HEADERS})
-SOURCE_GROUP("Header Files\\Private" FILES ${IDYNTREE_MODEL_PRIVATE_INCLUDES})
 
 set(libraryname idyntree-model)
 
-add_library(${libraryname} ${IDYNTREE_MODEL_SOURCES} ${IDYNTREE_MODEL_HEADERS} ${IDYNTREE_MODEL_PRIVATE_INCLUDES})
+add_library(${libraryname} ${IDYNTREE_MODEL_SOURCES} ${IDYNTREE_MODEL_HEADERS})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>"

From 626632ca21b9ac03cb03ed65f0d831860acafedb Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Thu, 19 Dec 2019 22:25:51 +0100
Subject: [PATCH 165/194] Update the changelog

---
 doc/releases/v0_12.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
index d62926f7988..71bc86fbeb8 100644
--- a/doc/releases/v0_12.md
+++ b/doc/releases/v0_12.md
@@ -74,6 +74,7 @@ to normalize errors or as initial points of a nonlinear optimization procedure.
 
 ### `Model`
 * Implement `getTotalMass()` method for Model class
+* Enable the installation of the `ModelTestUtils.h` file (https://github.com/robotology/idyntree/pull/607)
 
 ### `modelio`
 * Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).

From 68ee36c6fa8e1151992c4c348d1eed75e94b37df Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 17:05:59 +0100
Subject: [PATCH 166/194] Migrate changelog to use the structure defined in
 keepachangelog.com

The new  format is more compact, and permits to make releases without the overhead
of modifying an creating multiple files.
---
 CHANGELOG.md            | 200 ++++++++++++++++++++++++++++++++++++++++
 doc/releases/v0_10.md   |  38 --------
 doc/releases/v0_11.md   |  44 ---------
 doc/releases/v0_11_1.md |  14 ---
 doc/releases/v0_11_2.md |  32 -------
 doc/releases/v0_12.md   |  91 ------------------
 doc/releases/v0_8.md    |  28 ------
 doc/releases/v0_8_1.md  |  16 ----
 doc/releases/v0_8_2.md  |  12 ---
 9 files changed, 200 insertions(+), 275 deletions(-)
 create mode 100644 CHANGELOG.md
 delete mode 100644 doc/releases/v0_10.md
 delete mode 100644 doc/releases/v0_11.md
 delete mode 100644 doc/releases/v0_11_1.md
 delete mode 100644 doc/releases/v0_11_2.md
 delete mode 100644 doc/releases/v0_12.md
 delete mode 100644 doc/releases/v0_8.md
 delete mode 100644 doc/releases/v0_8_1.md
 delete mode 100644 doc/releases/v0_8_2.md

diff --git a/CHANGELOG.md b/CHANGELOG.md
new file mode 100644
index 00000000000..80a491c7acf
--- /dev/null
+++ b/CHANGELOG.md
@@ -0,0 +1,200 @@
+
+# Changelog
+All notable changes to this project will be documented in this file.
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/),
+and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
+
+## [Unreleased]
+
+### Added
+- Added method to compute the inverse dynamics inertial parameters regressor in KinDynComputations ( https://github.com/robotology/idyntree/pull/480 ).
+KinDynComputations finally reached feature parity with respect to DynamicsComputations, that will finally be removed in one of the future iDynTree feature releases.
+- Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).
+- Added objects to deal with linear optimal control problems in the optimalcontrol library.
+- Added ``OSQP`` interface via ``osqp-eigen`` in the optimalcontrol library.
+- Fixed bugs in ``MultipleShooting`` solver in the optimalcontrol library.
+- Added few lines of documentation in the optimalcontrol library.
+- Added interface for ``ALGLIB`` and ``WORHP`` in the optimalcontrol library.
+- Multiple shooting solvers can use hessians of costs and constraints in the optimalcontrol library.
+- Taking into account also the sparsity pattern of constraints and dynamical system (both in jacobians and hessians) in the optimalcontrol library.
+- Added visualization of vectors in the visualization library.
+- Added a SolidShape helper library. This library is part of iDynTree, and is meant
+to contain all the algorithms that use in some form the visual and collision geometries of the model,
+and so they depend on the Assimp library to load meshes.
+- Added an helper function that provides rough estimates of the inertial parameters (mass, first moments of mass,
+3d inertia matrix elements) of a robot given the total mass of the robot, and its collisions shapes. While the estimates
+provided are quite rough, they can be quite useful at least to provide an expected order of magnitude of the parameters,
+to normalize errors or as initial points of a nonlinear optimization procedure.
+- Added attitude estimator interface to estimate the orientation of an IMU, given the IMU measurements (https://github.com/robotology/idyntree/pull/516).
+- Added `DiscreteExtendedKalmanFilterHelper` base class (https://github.com/robotology/idyntree/pull/516).
+- Added `AttitudeMahonyFilter` implementation of an explicit formulation of passive complementary filter over quaternion groups (https://github.com/robotology/idyntree/pull/516).
+- Added `AttitudeQuaternionEKF` implementation (https://github.com/robotology/idyntree/pull/516).
+- Added `getWorldFrameTransform` implementation in `SimpleLeggedOdometry` class
+- Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame
+- Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
+- Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
+- Added `DiscreteKalmanFilterHelper` class for an implementation of a discrete, linear time-invariant Kalman Filter  (https://github.com/robotology/idyntree/pull/559)
+- Added dynamic reset functionality to `DiscreteExtendedKalmanFilterHelper` class (https://github.com/robotology/idyntree/pull/553)
+
+
+### Changed
+- The changelog has been migrated to the format described in https://keepachangelog.com/en/1.0.0/ .
+
+### Fixed
+- Fixed missing `DOF_ACCELLERATION` data in dynamic variable cache ordering
+(https://github.com/robotology/idyntree/pull/587)
+
+
+#### `estimation`
+
+#### `bindings`
+* Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)
+
+### `core`
+* Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)
+* Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)
+* Added basic tests for `Span` (https://github.com/robotology/idyntree/pull/522)
+* Implement `RPYRightTrivializedDerivativeRateOfChange()` and `RPYRightTrivializedDerivativeInverseRateOfChange()` into `Rotation` class
+* Implement left Jacobian and left Jacobian inverse of SO(3) in Rotation class (https://github.com/robotology/idyntree/pull/562)
+* Fixed implementation of Transform::log() method (https://github.com/robotology/idyntree/pull/562)
+* Fixed implementation of SpatialMotionVector::exp() method (https://github.com/robotology/idyntree/pull/562)
+* Add nameIsValid attribute to iDynTree::SolidShape class.
+* Add operator[] method to `iDynTree::VectorDynSize` (https://github.com/robotology/idyntree/pull/596)
+* Add operator[] method to `iDynTree::VectorFixSize` (https://github.com/robotology/idyntree/pull/596)
+
+### `Model`
+* Implement `getTotalMass()` method for Model class
+* Enable the installation of the `ModelTestUtils.h` file (https://github.com/robotology/idyntree/pull/607)
+
+### `modelio`
+* Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
+* Added support in the URDF parser to correctly parse the optional name parameter of visual and collision elements.
+* Added `iDynTree::ModelCalibrationHelper` to simplify loading a model from file, update its inertial parameters and exporting again to file (https://github.com/robotology/idyntree/pull/576).
+* Updated `iDynTree::ModelLoader` class to load by default models with normalized joint ordering (https://github.com/robotology/idyntree/issues/491).
+
+### `yarprobotstatepublisher`
+* Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
+* `robot` option is deprecated, and replaced by `name-prefix`.
+* Add `reduced-model` optional parameter to stream only the link transformations to transform server. By default, tranformations from all the frames are streamed to the transform server.
+* Remove joint size check between model joints and joints in ROS topic given through `jointstates-topic` parameter.
+* Initialize the model joint positions values to zero and update the joint positions values in run time if the values are
+available in ROS topic given through `jointstates-topic` parameter.
+
+## [0.11.2] - 2019-12-12
+
+### Added
+
+- The getFrameAcc method that returns the acceleration of a frame was added to the KinDynComputations class.
+  As this method takes in input every time the robot acceleration, it is computationally expensive and
+  is not suitable to be used for multiple frames in a tight loop. If you need a computationally convenient
+  method to access frame accelerations, please open an issue ( https://github.com/robotology/idyntree/pull/482 ).
+- It is now possible to specify a non-zero bias base acceleration as input of the ForwardBiasAccKinematics function.
+  This is convenient if the bias acceleration that is being computed is the bias acceleration obtained with the
+  MIXED velocity representation ( https://github.com/robotology/idyntree/pull/482 ).
+
+
+### Fixed
+
+- Fixed cache invalidation bug in the getFrameBiasAcc method of KinDynComputations. The internal
+  cache used by getBiasAcc was never updated even if the method setRobotState was called, so the
+  getFrameBiasAcc method always returned the bias acceleration corresponding to the first call to setRobotState ( https://github.com/robotology/idyntree/pull/482 ).
+- Fixed getBiasAcc method in KinDynComputations to take into account the effect of non-zero and non-parallel
+  linear and angular base velocity, described in https://github.com/robotology/idyntree/issues/370 .
+- Fixed compilation on 32-bit Windows ( https://github.com/robotology/idyntree/pull/506 )
+- Fixed a URDF parser regression introduced in iDynTree 0.11 for which collision geometries were also loaded as visual geometries, and viceversa (https://github.com/robotology/idyntree/issues/497, https://github.com/robotology/idyntree/pull/559).
+- Fixed a URDF parser regression introduced in iDynTree 0.11 for which geometry elements without origin tag were not correctly parsed (https://github.com/robotology/idyntree/issues/496, https://github.com/robotology/idyntree/pull/564).
+
+
+## [0.11.1] - 2018-09-10
+
+### Fixed
+
+- Fix a locale-dependent floating point parsing in the URDF parser (https://github.com/robotology/idyntree/pull/475).
+- Fix compatibility with SWIG 2 (https://github.com/robotology/idyntree/pull/476).
+- Fix behavior of InverseKinematics if Add***Constraint method are called multiple times (https://github.com/robotology/idyntree/pull/479).
+
+
+## [0.11.0] - 2018-08-31
+
+### Added
+- A new URDF parser was implemented. A side effect of the new parser is that the serialization of the links and joints of models
+  parsed from URDF may change. In your code, please use name string (in place of indices) to identify joints and links, or use the
+  `iDynTree::ModelLoader` class to load a Model with the specified joint serialization.
+- Added library `idyntree-modelio-xml` to parse generic XML files. Support for XSD validation (when parsing from file). This library requires [Gnome libxml2](http://xmlsoft.org).
+- Added `toEigen` methods for `Span` and added typedef for `Eigen` maps of vectors.
+- Added some typedefs to `VectorDynsize` and `VectorFixSize` classes to enable the use of `make_span` with these objects.
+- Added copy operator in `VectorDynSize` and `VectorFixSize` for `Span<double>`.
+- Added method to obtain relative Jacobians sparsity pattern.
+- Added method to obtain free floating Jacobians sparsity pattern.
+
+### Changed
+- iDynTree now official supports the following compilers: Microsoft Visual Studio 2015/2017, GCC >= 5.3 and clang >= 3.8 .
+- The C++14 standard language is now used in the project, including in the public headers.
+- [libxml2](http://xmlsoft.org) is now a required dependency. Check the docs to see how to install it.
+- CMake 3.5 is now required to build iDynTree.
+- Improve SpatialInertia documentation ( https://github.com/robotology/idyntree/pull/435 ).
+- Changed URDF parser to use `libxml2` instead of TinyXML.
+
+### Deprecated
+- The `iDynTree/Sensors/SixAxisFTSensor.h` header has been deprecated in favor of the `iDynTree/Sensors/SixAxisForceTorqueSensor.h`.
+- Constraints Jacobian now exploit sparsity pattern
+
+
+## [0.10.0] - 2018-06-20
+
+## Added
+
+- Added the `iDynTree::Span` class (https://github.com/robotology/idyntree/pull/434), modeled after
+  the C++20's `std::span` ( http://en.cppreference.com/w/cpp/container/span ). This class can be used to write
+  methods and functions that access a contiguous sequence of objects in memory with a known length.
+  This is extremly useful to make sure that `iDynTree` classes are easy to use regardless of which
+  vector type the downstream code is using, for example if it is one of `std::vector<double>`,
+  `Eigen::VectorXd` or `yarp::sig::Vector`
+- In the InverseKinematics library, the Frame Constraints can now be enabled and disabled dynamically ( https://github.com/robotology/idyntree/pull/389 ).
+- Addition of iDynTree::SchmittTrigger, iDynTree::ContactStateMachine and iDynTree::BipedFootContactClassifier classes for performing contact state detection using Schmitt trigger based thresholding and biped foot contact classification based on an alternate contact switching pattern reasoning over contact makes used for getting primary foot in contact
+  (https://github.com/robotology/idyntree/pull/410 ,   https://github.com/robotology/idyntree/pull/411 ).
+- Addition of iDynTree::GravityCompensationHelpers class for computing gravity compensation torques using accelerometer measurements (https://github.com/robotology/idyntree/pull/438)
+- Initial improvement of the optimal control library ( https://github.com/robotology/idyntree/pull/442 ). See the inline documentation of the classes for more details, or open an issue
+  ( https://github.com/robotology/idyntree/issues/new ) requesting documentation on some specific aspects of the optimal control library.
+
+
+## Changed
+- Since 0.10 release, iDynTree uses C++14 in its headers, and requires GCC >= 5 and Visual Studio >= 2015
+  to compile.
+- Similar to the YARP policy, any new version of iDynTree will have a consecutive minor version number.
+  Development version (the one contained in the `devel` branch before release will be denoted with a
+  patch version number greater than 100. The next minor release of iDynTree will be 0.11 .
+
+
+## [0.8.2] - 2018-06-20
+
+### Fixed
+
+- In the classes devoted to the external force-torque estimation, since https://github.com/robotology/idyntree/pull/343 it is possible to have external forces that are completly estimated from external sensors such as skin sensors. If in a given submodels there are no unknown due to this, the pseudoinverse should be skipped ( https://github.com/robotology/idyntree/pull/443 ) .
+- Fix compatibility with YARP 3.
+
+
+## [0.8.1] - 2017-09-29
+
+### Fixed
+- The `toEigen(const SpatialMotionVector & vec)` and `toEigen(const SpatialForceVector & vec)`  inline helpers functions in the `iDynTree/Core/EigenHelpers.h` have been modified to return a **`const`** copy of their content.
+While this is technically an API change, it was critical because most of the other `toEigen` methods return an `Eigen::Map` object, and users
+expect to be able to use `Eigen` modifiers methods such as `toEigen(...).setZero()`. Enforcing a compilation error in this case will help to prevent subtle bugs.
+As this is an `inline` function, this modification does not affect `iDynTree`'s `ABI` ( https://github.com/robotology/idyntree/pull/378 ) .
+- The CMake configuration files of the release `0.8.0` were generated with the wrong version `0.7.2`.
+
+## [0.8.0] - 2017-09-25
+
+### Added
+
+- This is the first version of iDynTree for which a changelog was introduced.
+- The iDynTree::Model class now supports prismatic joints, using the iDynTree::PrismaticJoint class. Support for prismatic joints was added also to the URDF parser ( https://github.com/robotology/idyntree/pull/269 ).
+- Classes and function to convert between chains rappresented by the Denavit Hartnberg convention and iDynTree::Model object have been implemented ( https://github.com/robotology/idyntree/pull/350 ).
+
+### Changed
+
+- The methods and constructors of the iDynTree::RevoluteJoint and iDynTreee::PrismaticJoint classes handling joints have been cleaned up ( https://github.com/robotology/idyntree/pull/339 ).
+- The logic to enable/disable dependencies has changed. In particular, now all the dependencies (excluding the legacy dependency on KDL) that are found on the system are enabled. Users can still select manually the dependency that they want to compile using the `IDYNTREE_USES_<dep>` variables ( https://github.com/robotology/idyntree/pull/323 ).
+- IPOPT has been added as an optional dependency for the Inverse Kinematics component.
+- The Octave bindings have been migrated to use the exact same mex code used for the Matlab bindings ( https://github.com/robotology/idyntree/pull/305 ).
diff --git a/doc/releases/v0_10.md b/doc/releases/v0_10.md
deleted file mode 100644
index ed5c62cced4..00000000000
--- a/doc/releases/v0_10.md
+++ /dev/null
@@ -1,38 +0,0 @@
-iDynTree 0.10 (2018-06-20)                                              {#v0_10}
-========================
-
-[TOC]
-
-iDynTree 0.10 Release Notes
-=========================
-
-
-Important Changes
------------------
-* Since 0.10 release, iDynTree uses C++14 in its headers, and requires GCC >= 5 and Visual Studio >= 2015
-  to compile.
-* Similar to the YARP policy, any new version of iDynTree will have a consecutive minor version number.
-  Development version (the one contained in the `devel` branch before release will be denoted with a
-  patch version number greater than 100. The next minor release of iDynTree will be 0.11 .
-
-
-#### `core`
-* Added the `iDynTree::Span` class (https://github.com/robotology/idyntree/pull/434), modeled after
-  the C++20's `std::span` ( http://en.cppreference.com/w/cpp/container/span ). This class can be used to write
-  methods and functions that access a contiguous sequence of objects in memory with a known length.
-  This is extremly useful to make sure that `iDynTree` classes are easy to use regardless of which
-  vector type the downstream code is using, for example if it is one of `std::vector<double>`,
-  `Eigen::VectorXd` or `yarp::sig::Vector` .
-
-#### `inverse-kinematics`
-
-* Frame Constraints can now be enabled and disabled dynamically ( https://github.com/robotology/idyntree/pull/389 ).
-
-#### `estimation`
-* Addition of iDynTree::SchmittTrigger, iDynTree::ContactStateMachine and iDynTree::BipedFootContactClassifier classes for performing contact state detection using Schmitt trigger based thresholding and biped foot contact classification based on an alternate contact switching pattern reasoning over contact makes used for getting primary foot in contact
-  (https://github.com/robotology/idyntree/pull/410 ,   https://github.com/robotology/idyntree/pull/411 ).
-* Addition of iDynTree::GravityCompensationHelpers class for computing gravity compensation torques using accelerometer measurements (https://github.com/robotology/idyntree/pull/438)
-
-#### `optimalcontrol`
-* Initial improvement of the optimal control library ( https://github.com/robotology/idyntree/pull/442 ). See the inline documentation of the classes for more details, or open an issue
-  ( https://github.com/robotology/idyntree/issues/new ) requesting documentation on some specific aspects of the optimal control library.
diff --git a/doc/releases/v0_11.md b/doc/releases/v0_11.md
deleted file mode 100644
index 9fadfbbd232..00000000000
--- a/doc/releases/v0_11.md
+++ /dev/null
@@ -1,44 +0,0 @@
-iDynTree 0.11 (2018-08-31)                                              {#v0_11}
-========================
-
-[TOC]
-
-iDynTree 0.11 Release Notes
-=========================
-
-Summary
--------
-* iDynTree now official supports the following compilers: Microsoft Visual Studio 2015/2017, GCC >= 5.3 and clang >= 3.8 .
-* The C++14 standard language is now used in the project, including in the public headers.
-* [libxml2](http://xmlsoft.org) is now a required dependency. Check the docs to see how to install it.
-* A new URDF parser was implemented. A side effect of the new parser is that the serialization of the links and joints of models
-  parsed from URDF may change. In your code, please use name string (in place of indices) to identify joints and links, or use the
-  `iDynTree::ModelLoader` class to load a Model with the specified joint serialization.
-* CMake 3.5 is now required to build iDynTree.
-
-Important Changes
------------------
-
-### `core`
-* Improve SpatialInertia documentation ( https://github.com/robotology/idyntree/pull/435 ).
-
-### `sensors`
-* The `iDynTree/Sensors/SixAxisFTSensor.h` header has been deprecated in favor of the `iDynTree/Sensors/SixAxisForceTorqueSensor.h`.
-
-#### `model_io`
-* Added dependency on [Gnome libxml2](http://xmlsoft.org), see next bullet point.
-* Added library `idyntree-modelio-xml` to parse generic XML files. Support for XSD validation (when parsing from file). This library requires [Gnome libxml2](http://xmlsoft.org).
-* Rewritten URDF parser to use the new `idyntree-modelio-xml` library.
-* Dropped required dependency on TinyXML1. If TinyXML is found in the system, it is possible to compile the old URDF parser that has been renamed into `idyntree-modelio-urdf-legacy`.
-
-#### `core`
-* Added `toEigen` methods for `Span` and added typedef for `Eigen` maps of vectors.
-* Added some typedefs to `VectorDynsize` and `VectorFixSize` classes to enable the use of `make_span` with these objects.
-* Added copy operator in `VectorDynSize` and `VectorFixSize` for `Span<double>`.
-
-#### `high-level`
-* Added method to obtain relative Jacobians sparsity pattern
-* Added method to obtain free floating Jacobians sparsity pattern
-
-#### `inverse-kinematics`
-* Constraints Jacobian now exploit sparsity pattern
diff --git a/doc/releases/v0_11_1.md b/doc/releases/v0_11_1.md
deleted file mode 100644
index 25f79240b79..00000000000
--- a/doc/releases/v0_11_1.md
+++ /dev/null
@@ -1,14 +0,0 @@
-iDynTree 0.11.1 (2018-09-10)                                              {#v0_11_1}
-========================
-
-[TOC]
-
-iDynTree 0.11.1 Release Notes
-=========================
-
-Bug Fixes
----------
-
-* Fix a locale-dependent floating point parsing in the URDF parser (https://github.com/robotology/idyntree/pull/475).
-* Fix compatibility with SWIG 2 (https://github.com/robotology/idyntree/pull/476).
-* Fix behavior of InverseKinematics if Add***Constraint method are called multiple times (https://github.com/robotology/idyntree/pull/479).
diff --git a/doc/releases/v0_11_2.md b/doc/releases/v0_11_2.md
deleted file mode 100644
index 38841673c9d..00000000000
--- a/doc/releases/v0_11_2.md
+++ /dev/null
@@ -1,32 +0,0 @@
-iDynTree 0.11.2 (2019-12-12)                                              {#v0_11_2}
-========================
-
-[TOC]
-
-iDynTree 0.11.2 Release Notes
-=========================
-
-Unless noted otherwise, the changes in this patch release were introduced in Pull Request https://github.com/robotology/idyntree/pull/482 .
-
-Bug Fixes
----------
-* Fixed cache invalidation bug in the getFrameBiasAcc method of KinDynComputations. The internal
-  cache used by getBiasAcc was never updated even if the method setRobotState was called, so the
-  getFrameBiasAcc method always returned the bias acceleration corresponding to the first call to setRobotState.
-* Fixed getBiasAcc method in KinDynComputations to take into account the effect of non-zero and non-parallel
-  linear and angular base velocity, described in https://github.com/robotology/idyntree/issues/370 .
-* Fixed compilation on 32-bit Windows ( https://github.com/robotology/idyntree/pull/506 )
-* Fixed a URDF parser regression introduced in iDynTree 0.11 for which collision geometries were also loaded as visual geometries, and viceversa (https://github.com/robotology/idyntree/issues/497, https://github.com/robotology/idyntree/pull/559).
-* Fixed a URDF parser regression introduced in iDynTree 0.11 for which geometry elements without origin tag were not correctly parsed (https://github.com/robotology/idyntree/issues/496, https://github.com/robotology/idyntree/pull/564).
-
-New features
-------------
-* The getFrameAcc method that returns the acceleration of a frame was added to the KinDynComputations class.
-  As this method takes in input every time the robot acceleration, it is computationally expensive and
-  is not suitable to be used for multiple frames in a tight loop. If you need a computationally convenient
-  method to access frame accelerations, please open an issue.
-* It is now possible to specify a non-zero bias base acceleration as input of the ForwardBiasAccKinematics function.
-  This is convenient if the bias acceleration that is being computed is the bias acceleration obtained with the
-  MIXED velocity representation.
-
-
diff --git a/doc/releases/v0_12.md b/doc/releases/v0_12.md
deleted file mode 100644
index 71bc86fbeb8..00000000000
--- a/doc/releases/v0_12.md
+++ /dev/null
@@ -1,91 +0,0 @@
-iDynTree 0.12 (UNRELEASED)                                              {#v0_12}
-========================
-
-[TOC]
-
-iDynTree 0.12 Release Notes
-=========================
-
-Summary
--------
-*
-
-Important Changes
------------------
-
-#### `high-level`
-* Added method to compute the inverse dynamics inertial parameters regressor in KinDynComputations ( https://github.com/robotology/idyntree/pull/480 ).
-KinDynComputations finally reached feature parity with respect to DynamicsComputations, that will finally be removed in one of the future iDynTree feature releases.
-
-#### `inverse-kinematics`
-* Added method to return the convex hull of the constraint on the projection of the center of mass (https://github.com/robotology/idyntree/pull/478).
-
-#### `optimalcontrol`
-* Added objects to deal with linear optimal control problems.
-* Added ``OSQP`` interface via ``osqp-eigen``.
-* Fixed bugs in ``MultipleShooting`` solver.
-* Added few lines of documentation.
-* Added interface for ``ALGLIB`` and ``WORHP``.
-* Multiple shooting solvers can use hessians of costs and constraints
-* Taking into account also the sparsity pattern of constraints and dynamical system (both in jacobians and hessians).
-
-#### `visualization`
-* Added visualization of vectors.
-
-#### `solid-shapes`
-* Added a SolidShape helper library. This library is part of iDynTree, and is meant
-to contain all the algorithms that use in some form the visual and collision geometries of the model,
-and so they depend on the Assimp library to load meshes.
-* Added an helper function that provides rough estimates of the inertial parameters (mass, first moments of mass,
-3d inertia matrix elements) of a robot given the total mass of the robot, and its collisions shapes. While the estimates
-provided are quite rough, they can be quite useful at least to provide an expected order of magnitude of the parameters,
-to normalize errors or as initial points of a nonlinear optimization procedure.
-
-
-#### `estimation`
-* Added attitude estimator interface to estimate the orientation of an IMU, given the IMU measurements
-* Added `DiscreteExtendedKalmanFilterHelper` base class
-* Added `AttitudeMahonyFilter` implementation of an explicit formulation of passive complementary filter over quaternion groups
-* Added `AttitudeQuaternionEKF` implementation
-* All of the above addressed in the PR (https://github.com/robotology/idyntree/pull/516)
-* Added `getWorldFrameTransform` implementation in `SimpleLeggedOdometry` class
-* Added a new version of `changeFixedFrame` in `SimpleLeggedOdometry`class. This can be used to set a desired homogeneous transformation for the fixed frame
-* Added `bindings` for `AttitudeMahonyFilter`, `AttitudeQuaternionEKF`, `DiscreteExtendedKalmanFilterHelper` (https://github.com/robotology/idyntree/pull/522)
-* Added basic tests for the Attitude Estimator classes (https://github.com/robotology/idyntree/pull/522)
-* Added `DiscreteKalmanFilterHelper` class for an implementation of a discrete, linear time-invariant Kalman Filter  (https://github.com/robotology/idyntree/pull/559)
-* Added dynamic reset functionality to `DiscreteExtendedKalmanFilterHelper` class (https://github.com/robotology/idyntree/pull/553)
-* Fixed missing `DOF_ACCELLERATION` data in dynamic variable cache ordering
-(https://github.com/robotology/idyntree/pull/587)
-
-#### `bindings`
-* Added high-level Matlab/Octave wrappers of the iDyntree bindings (https://github.com/robotology/idyntree/pull/530)
-
-### `core`
-* Fixed compatibility of `Span` with SWIG bindings compilation (https://github.com/robotology/idyntree/pull/522)
-* Added bindings for the class `Span` with the name `DynamicSpan` (https://github.com/robotology/idyntree/pull/522)
-* Added basic tests for `Span` (https://github.com/robotology/idyntree/pull/522)
-* Implement `RPYRightTrivializedDerivativeRateOfChange()` and `RPYRightTrivializedDerivativeInverseRateOfChange()` into `Rotation` class
-* Implement left Jacobian and left Jacobian inverse of SO(3) in Rotation class (https://github.com/robotology/idyntree/pull/562)
-* Fixed implementation of Transform::log() method (https://github.com/robotology/idyntree/pull/562)
-* Fixed implementation of SpatialMotionVector::exp() method (https://github.com/robotology/idyntree/pull/562)
-* Add nameIsValid attribute to iDynTree::SolidShape class.
-* Add operator[] method to `iDynTree::VectorDynSize` (https://github.com/robotology/idyntree/pull/596)
-* Add operator[] method to `iDynTree::VectorFixSize` (https://github.com/robotology/idyntree/pull/596)
-
-### `Model`
-* Implement `getTotalMass()` method for Model class
-* Enable the installation of the `ModelTestUtils.h` file (https://github.com/robotology/idyntree/pull/607)
-
-### `modelio`
-* Added `iDynTree::ModelExporter` class to export `iDynTree::Model` instances to URDF files (https://github.com/robotology/idyntree/pull/554).
-* Added support in the URDF parser to correctly parse the optional name parameter of visual and collision elements.
-* Added `iDynTree::ModelCalibrationHelper` to simplify loading a model from file, update its inertial parameters and exporting again to file (https://github.com/robotology/idyntree/pull/576).
-* Updated `iDynTree::ModelLoader` class to load by default models with normalized joint ordering (https://github.com/robotology/idyntree/issues/491).
-
-### `yarprobotstatepublisher`
-* Add `tf-prefix` and `jointstates-topic` options for the tf prefixes and ROS topic.
-* `robot` option is deprecated, and replaced by `name-prefix`.
-* Add `reduced-model` optional parameter to stream only the link transformations to transform server. By default, tranformations from all the frames are streamed to the transform server.
-* Remove joint size check between model joints and joints in ROS topic given through `jointstates-topic` parameter.
-* Initialize the model joint positions values to zero and update the joint positions values in run time if the values are
-available in ROS topic given through `jointstates-topic` parameter.
diff --git a/doc/releases/v0_8.md b/doc/releases/v0_8.md
deleted file mode 100644
index 742ac6929fa..00000000000
--- a/doc/releases/v0_8.md
+++ /dev/null
@@ -1,28 +0,0 @@
-iDynTree 0.8 (2017-09-25)                                              {#v0_8}
-========================
-
-[TOC]
-
-iDynTree 0.8 Release Notes
-=========================
-
-This is the first version of iDynTree for which a changelog was introduced. 
-
-Important Changes
------------------
-* The iDynTree::Model class now supports prismatic joints, using the iDynTree::PrismaticJoint class. Support for prismatic joints was added also to the URDF parser ( https://github.com/robotology/idyntree/pull/269 ). 
-
-* The methods and constructors of the iDynTree::RevoluteJoint and iDynTreee::PrismaticJoint classes handling joints have been cleaned up ( https://github.com/robotology/idyntree/pull/339 ).
-
-* Classes and function to convert between chains rappresented by the Denavit Hartnberg convention and iDynTree::Model object have been implemented ( https://github.com/robotology/idyntree/pull/350 ).
-
-### Dependencies
-
-* The logic to enable/disable dependencies has changed. In particular, now all the dependencies (excluding the legacy dependency on KDL)
-  that are found on the system are enabled. Users can still select manually the dependency that they want to compile using 
-  the `IDYNTREE_USES_<dep>` variables ( https://github.com/robotology/idyntree/pull/323 ).
-* IPOPT has been added as an optional dependency for the Inverse Kinematics component.
-
-### Bindings 
-
-* The Octave bindings have been migrated to use the exact same mex code used for the Matlab bindings ( https://github.com/robotology/idyntree/pull/305 ). 
diff --git a/doc/releases/v0_8_1.md b/doc/releases/v0_8_1.md
deleted file mode 100644
index ac9e3611e90..00000000000
--- a/doc/releases/v0_8_1.md
+++ /dev/null
@@ -1,16 +0,0 @@
-iDynTree 0.8.1 (2017-09-29)                                              {#v0_8_1}
-========================
-
-[TOC]
-
-iDynTree 0.8.1 Release Notes
-=========================
-
-Bug Fixes
----------
-* The `toEigen(const SpatialMotionVector & vec)` and `toEigen(const SpatialForceVector & vec)`  inline helpers functions in the `iDynTree/Core/EigenHelpers.h` have been modified to return a **`const`** copy of their content. 
-While this is technically an API change, it was critical because most of the other `toEigen` methods return an `Eigen::Map` object, and users 
-expect to be able to use `Eigen` modifiers methods such as `toEigen(...).setZero()`. Enforcing a compilation error in this case will help to prevent subtle bugs.
-As this is an `inline` function, this modification does not affect `iDynTree`'s `ABI` ( https://github.com/robotology/idyntree/pull/378 ) .
-
-* The CMake configuration files of the release `0.8.0` were generated with the wrong version `0.7.2`. 
\ No newline at end of file
diff --git a/doc/releases/v0_8_2.md b/doc/releases/v0_8_2.md
deleted file mode 100644
index 2f0852fdcbc..00000000000
--- a/doc/releases/v0_8_2.md
+++ /dev/null
@@ -1,12 +0,0 @@
-iDynTree 0.8.2 (2018-06-20)                                              {#v0_8_2}
-========================
-
-[TOC]
-
-iDynTree 0.8.2 Release Notes
-=========================
-
-Bug Fixes
----------
-* In the classes devoted to the external force-torque estimation, since https://github.com/robotology/idyntree/pull/343 it is possible to have external forces that are completly estimated from external sensors such as skin sensors. If in a given submodels there are no unknown due to this, the pseudoinverse should be skipped ( https://github.com/robotology/idyntree/pull/443 ) .
-* Fix compatibility with YARP 3. 

From d22c641147d0af071b004f6921b439b86cf851bf Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 17:09:41 +0100
Subject: [PATCH 167/194] Delete appveyor.yml

---
 appveyor.yml | 52 ----------------------------------------------------
 1 file changed, 52 deletions(-)
 delete mode 100644 appveyor.yml

diff --git a/appveyor.yml b/appveyor.yml
deleted file mode 100644
index 31b2c3f9c0d..00000000000
--- a/appveyor.yml
+++ /dev/null
@@ -1,52 +0,0 @@
-version: 1.0.{build}
-
-clone_folder: c:\projects\idyntree
-
-environment:
-  Eigen3_DIR:          C:/Program Files (x86)/Eigen/lib/cmake/eigen3/
-  CMAKE_PREFIX_PATH:   c:\projects\rad\robotology-additional-dependencies-0.1.0-x86\x86-windows\;c:\projects\rad\robotology-additional-dependencies-0.1.0-x86\x86-windows\debug
-
-os:
- - Visual Studio 2015
- - Visual Studio 2017
-
-install:
-  # download and install libxml2 
-  - ps: Start-FileDownload https://github.com/robotology-playground/robotology-additional-dependencies/releases/download/v0.1.0/robotology-additional-dependencies-0.1.0-v14-x86.zip -FileName c:\projects\rad.zip -Timeout 300000
-  - ps: Expand-Archive c:\projects\rad.zip -DestinationPath c:\projects\rad
-  # Check env variables 
-  - cmd: echo CMAKE_PREFIX_PATH %CMAKE_PREFIX_PATH%
-  - cmd: set PATH=%PATH%;c:\projects\rad\robotology-additional-dependencies-0.1.0-x86\x86-windows\bin;c:\projects\rad\robotology-additional-dependencies-0.1.0-x86\x86-windows\debug\bin
-  - cmd: echo PATH %PATH%
-build:
-
-build_script:
-  # download and build tinyxml
-  - cd c:\projects
-  - git clone https://github.com/robotology-dependencies/tinyxml
-  - cd tinyxml
-  - md build
-  - cd build
-  - cmake ..
-  - msbuild /m /p:Configuration=Release /p:Platform="Win32" tinyxml.sln
-  - cmake --build . --config Release
-  - cmake --build . --config Release --target INSTALL
-  # download and install eigen3
-  - cd c:\projects
-  - hg clone https://bitbucket.org/eigen/eigen
-  - cd eigen
-  - hg checkout 3.3-beta2
-  - md build
-  - cd build
-  - cmake ..
-  - cmake --build . --config Release --target INSTALL
-  # compile iDynTree
-  - cd c:\projects\idyntree
-  - md build
-  - cd build
-  - cmake .. -DEIGEN3_INCLUDE_DIR="C:/Program Files (x86)/Eigen/include/eigen3" -DIDYNTREE_USES_KDL:BOOL=OFF -DIDYNTREE_USES_IPOPT:BOOL=OFF -DIDYNTREE_USES_YARP:BOOL=OFF -DIDYNTREE_USES_ICUB_MAIN:BOOL=OFF -DIDYNTREE_COMPILE_TESTS:BOOL=ON
-  - msbuild /m /p:Configuration=Release /p:Platform="Win32" iDynTree.sln
-  - cmake --build . --config Release
-  - ctest --output-on-failure --build-config Release
-  - cmake --build . --config Release --target INSTALL
-

From 78929096d57ea14beb47b52240f950cbeabb8503 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 17:23:52 +0100
Subject: [PATCH 168/194] Update README.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index a5b10711231..7692843cff0 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-iDynTree [![Build Status](https://travis-ci.org/robotology/idyntree.svg?branch=maste2013-r)](https://travis-ci.org/robotology/idyntree) [![Build status](https://ci.appveyor.com/api/projects/status/1uecfmyvxb2dujt9/branch/master?svg=true)](https://ci.appveyor.com/project/robotology/idyntree/branch/master) [![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0) [![License: LGPL v2](https://img.shields.io/badge/License-LGPL%20v2-blue.svg)](https://www.gnu.org/licenses/lgpl-2.1)  [![ZenHub](https://img.shields.io/badge/Shipping_faster_with-ZenHub-435198.svg)](https://zenhub.com)
+iDynTree [![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0) [![License: LGPL v2](https://img.shields.io/badge/License-LGPL%20v2-blue.svg)](https://www.gnu.org/licenses/lgpl-2.1)  [![ZenHub](https://img.shields.io/badge/Shipping_faster_with-ZenHub-435198.svg)](https://zenhub.com)
 ===========
 
 iDynTree is a library of robots dynamics algorithms for control, estimation and simulation.

From 81b2ed1d64d79dae0ceb9983a71a665eafede22b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 17:40:47 +0100
Subject: [PATCH 169/194] Remove duplicated isFakeLinks and removeFakeLinks
 from the URDF parser

Fix https://github.com/robotology/idyntree/issues/556
---
 src/model_io/urdf/src/URDFDocument.cpp | 121 +------------------------
 1 file changed, 1 insertion(+), 120 deletions(-)

diff --git a/src/model_io/urdf/src/URDFDocument.cpp b/src/model_io/urdf/src/URDFDocument.cpp
index 3690b8ec219..177f1f59f06 100644
--- a/src/model_io/urdf/src/URDFDocument.cpp
+++ b/src/model_io/urdf/src/URDFDocument.cpp
@@ -27,8 +27,6 @@ namespace iDynTree {
     static std::unordered_set<std::string> processJoints(iDynTree::Model& model,
                                                          std::unordered_map<std::string, JointElement::JointInfo>& joints,
                                                          std::unordered_map<std::string, JointElement::JointInfo>& fixed_joints);
-    static bool isFakeLink(const iDynTree::Model& modelWithFakeLinks, const iDynTree::LinkIndex linkToCheck);
-    static bool p_removeFakeLinks(const iDynTree::Model &originalModel, iDynTree::Model& cleanModel);
     static bool processSensors(const Model& model,
                                const std::vector<std::shared_ptr<SensorHelper>>& helpers,
                                iDynTree::SensorsList& sensors);
@@ -122,7 +120,7 @@ namespace iDynTree {
         m_model.setDefaultBaseLink(m_model.getLinkIndex(rootCandidates[0]));
         iDynTree::Model newModel, normalizedModel;
 
-        if (!p_removeFakeLinks(m_model, newModel)) {
+        if (!removeFakeLinks(m_model, newModel)) {
             reportError("URDFDocument", "documentHasBeenParsed", "Failed to remove fake links from the model");
             return false;
         }
@@ -219,123 +217,6 @@ namespace iDynTree {
         return childLinks;
     }
 
-
-    bool isFakeLink(const Model& modelWithFakeLinks, const LinkIndex linkToCheck)
-    {
-        // First condition: base link is massless
-        double mass = modelWithFakeLinks.getLink(linkToCheck)->getInertia().getMass();
-        if (mass > 0.0)
-        {
-            return false;
-        }
-
-        // Second condition: the base link has only one child
-        if (modelWithFakeLinks.getNrOfNeighbors(linkToCheck) != 1)
-        {
-            return false;
-        }
-
-        // Third condition: the base link is attached to its child with a fixed joint
-        Neighbor neigh = modelWithFakeLinks.getNeighbor(linkToCheck, 0);
-        if (modelWithFakeLinks.getJoint(neigh.neighborJoint)->getNrOfDOFs() > 0)
-        {
-            return false;
-        }
-
-        return true;
-    }
-
-    bool p_removeFakeLinks(const iDynTree::Model &originalModel, iDynTree::Model& cleanModel)
-    {
-        // Clear the output model
-        cleanModel = iDynTree::Model();
-
-        std::unordered_set<std::string> linksToRemove;
-        std::unordered_set<std::string> jointsToRemove;
-
-        std::string newDefaultBaseLink = originalModel.getLinkName(originalModel.getDefaultBaseLink());
-
-        // We iterate on all the links in the model
-        // and check which one are "fake links", according
-        // to our definition
-        for (LinkIndex linkIndex = 0; linkIndex < (LinkIndex)originalModel.getNrOfLinks(); ++linkIndex)
-        {
-            if (isFakeLink(originalModel, linkIndex))
-            {
-                linksToRemove.insert(originalModel.getLinkName(linkIndex));
-                JointIndex jointIndex = originalModel.getNeighbor(linkIndex, 0).neighborJoint;
-                jointsToRemove.insert(originalModel.getJointName(jointIndex));
-
-                // if the fake link is the default base, we also need to update the
-                // default base in the new model
-                if (linkIndex == originalModel.getDefaultBaseLink())
-                {
-                    LinkIndex newBaseIndex = originalModel.getNeighbor(linkIndex, 0).neighborLink;
-                    newDefaultBaseLink = originalModel.getLinkName(newBaseIndex);
-                }
-            }
-        }
-
-        // Add all links, except for the one that we need to remove
-        for (unsigned link = 0; link < originalModel.getNrOfLinks(); ++link)
-        {
-            std::string linkToAdd = originalModel.getLinkName(link);
-            if (linksToRemove.find(linkToAdd) == linksToRemove.end())
-            {
-                cleanModel.addLink(linkToAdd, *originalModel.getLink(link));
-            }
-        }
-
-        // Add all joints, preserving the serialization
-        for (unsigned joint = 0; joint < originalModel.getNrOfJoints(); ++joint)
-        {
-            std::string jointToAdd = originalModel.getJointName(joint);
-            if (jointsToRemove.find(jointToAdd) == jointsToRemove.end())
-            {
-                // we need to change the link index in the new joints
-                // to match the new link serialization
-                IJointPtr newJoint = originalModel.getJoint(joint)->clone();
-                std::string firstLinkName = originalModel.getLinkName(newJoint->getFirstAttachedLink());
-                std::string secondLinkName = originalModel.getLinkName(newJoint->getSecondAttachedLink());
-                JointIndex firstLinkNewIndex = cleanModel.getLinkIndex(firstLinkName);
-                JointIndex secondLinkNewIndex = cleanModel.getLinkIndex(secondLinkName);
-                newJoint->setAttachedLinks(firstLinkNewIndex, secondLinkNewIndex);
-
-                cleanModel.addJoint(jointToAdd, newJoint);
-                delete newJoint;
-            }
-        }
-
-        // Then we add all frames (i.e. fake links that we removed from the model)
-        for (unsigned link = 0; link < originalModel.getNrOfLinks(); ++link)
-        {
-            std::string fakeLinkName = originalModel.getLinkName(link);
-            if (linksToRemove.find(fakeLinkName) != linksToRemove.end())
-            {
-                LinkIndex fakeLinkOldIndex = originalModel.getLinkIndex(fakeLinkName);
-
-                // One of the condition for a base to be fake is to
-                // be connected to the real link with a fixed joint, so
-                // their transform can be obtained without specifying the joint positions
-                assert(originalModel.getNrOfNeighbors(fakeLinkOldIndex) == 1);
-
-                JointIndex fakeLink_realLink_joint = originalModel.getNeighbor(fakeLinkOldIndex, 0).neighborJoint;
-                LinkIndex realLinkOldIndex = originalModel.getNeighbor(fakeLinkOldIndex, 0).neighborLink;
-                std::string realLinkName = originalModel.getLinkName(realLinkOldIndex);
-
-                // Get the transform
-                iDynTree::Transform realLink_H_fakeLink =
-                originalModel.getJoint(fakeLink_realLink_joint)->getRestTransform(realLinkOldIndex, fakeLinkOldIndex);
-
-                // Add the fake base as a frame
-                cleanModel.addAdditionalFrameToLink(realLinkName, fakeLinkName, realLink_H_fakeLink);
-            }
-        }
-
-        // Set the default base link
-        return cleanModel.setDefaultBaseLink(cleanModel.getLinkIndex(newDefaultBaseLink));
-    }
-
     bool processSensors(const Model& model,
                         const std::vector<std::shared_ptr<SensorHelper>>& helpers,
                         iDynTree::SensorsList& sensors)

From 53259acd12b585df1fe21e7e0637ccb29b674f60 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 17:57:40 +0100
Subject: [PATCH 170/194] Update vendored YCM files to latest commit

In particular, update to commit https://github.com/robotology/ycm/commit/597f2b1e800d8381b5e14a33274cf03850365aa4 .

The major changes is that now it should be supported to use iDynTree via add_subdirectory,
fixing https://github.com/robotology/idyntree/issues/397 .

Furthermore, now the CMake config files of iDynTree will always be installed in
${CMAKE_INSTALL_PREFIX}/lib/cmake/iDynTree also on Windows.
---
 CMakeLists.txt                        |  23 +-
 cmake/AddInstallRPATHSupport.cmake    | 120 +++--
 cmake/AddUninstallTarget.cmake        |  81 ++-
 cmake/CMakePackageConfigHelpers.cmake | 321 -----------
 cmake/InstallBasicPackageFiles.cmake  | 747 +++++++++++++++++++-------
 iDynTreeConfig.cmake.in               |  36 --
 6 files changed, 705 insertions(+), 623 deletions(-)
 delete mode 100644 cmake/CMakePackageConfigHelpers.cmake
 delete mode 100644 iDynTreeConfig.cmake.in

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 691ef72e7eb..09acb723216 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,12 +63,27 @@ if(NOT IDYNTREE_ONLY_DOCS)
     # add the actual components of the library
     add_subdirectory(src)
 
+    # List exported dependencies
+    set(_IDYNTREE_EXPORTED_DEPENDENCIES "")
+    if(IDYNTREE_USES_YARP)
+        list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES YARP)
+    endif()
+    if(IDYNTREE_USES_ICUB_MAIN)
+        list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES ICUB)
+    endif()
+    if(IDYNTREE_USES_KDL)
+        list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES orocos_kdl)
+        list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES TinyXML)
+    endif()
+
     include(InstallBasicPackageFiles)
     install_basic_package_files(iDynTree VARS_PREFIX ${VARS_PREFIX}
-                                        VERSION ${${VARS_PREFIX}_VERSION}
-                                        COMPATIBILITY SameMajorVersion
-                                        TARGETS_PROPERTY ${VARS_PREFIX}_TARGETS
-                                        NO_CHECK_REQUIRED_COMPONENTS_MACRO)
+                                         VERSION ${${VARS_PREFIX}_VERSION}
+                                         COMPATIBILITY SameMajorVersion
+                                         TARGETS_PROPERTY ${VARS_PREFIX}_TARGETS
+                                         NO_CHECK_REQUIRED_COMPONENTS_MACRO
+                                         ENABLE_COMPATIBILITY_VARS
+                                         DEPENDENCIES ${_IDYNTREE_EXPORTED_DEPENDENCIES})
 
     include(AddUninstallTarget)
 
diff --git a/cmake/AddInstallRPATHSupport.cmake b/cmake/AddInstallRPATHSupport.cmake
index a69347ced96..c36cc2072d2 100644
--- a/cmake/AddInstallRPATHSupport.cmake
+++ b/cmake/AddInstallRPATHSupport.cmake
@@ -6,7 +6,8 @@
 #
 #   add_install_rpath_support([BIN_DIRS dir [dir]]
 #                             [LIB_DIRS dir [dir]]
-#                             [DEPENDS condition]
+#                             [INSTALL_NAME_DIR [dir]]
+#                             [DEPENDS condition [condition]]
 #                             [USE_LINK_PATH])
 #
 # Normally (depending on the platform) when you install a shared
@@ -28,7 +29,7 @@
 # - the variable will be used only by applications spawned by the shell
 #   and not by other processes.
 #
-# RPATH is aimed to solve the issues introduced by the second
+# RPATH aims in solving the issues introduced by the second
 # installation method. Using run-path dependent libraries you can
 # create a directory structure containing executables and dependent
 # libraries that users can relocate without breaking it.
@@ -40,30 +41,41 @@
 # hardcode in the binary itself the additional search directories
 # to be passed to the dynamic linker. This works great in conjunction
 # with relative paths.
-# This macro will enable support to RPATH to your project.
+# This command will enable support to RPATH to your project.
 # It will enable the following things:
 #
 #  - If the project builds shared libraries it will generate a run-path
 #    enabled shared library, i.e. its install name will be resolved
 #    only at run time.
 #  - In all cases (building executables and/or shared libraries)
-#    dependent shared libraries with RPATH support will be properly
+#    dependent shared libraries with RPATH support will have their name
+#    resolved only at run time, by embedding the search path directly
+#    into the built binary.
 #
-# The macro has the following parameters:
+# The command has the following parameters:
 #
 # Options:
-#  - ``USE_LINK_PATH``: if passed the macro will automatically adds to
-#    the RPATH the path to all the dependent libraries
+#  - ``USE_LINK_PATH``: if passed the command will automatically adds to
+#    the RPATH the path to all the dependent libraries.
 #
 # Arguments:
-#  - ``BIN_DIRS`` list of directories when the targets (bins or shared
-#    libraries) will be installed
+#  - ``BIN_DIRS`` list of directories when the targets (executable and
+#    plugins) will be installed.
 #  - ``LIB_DIRS`` list of directories to be added to the RPATH. These
-#    directories will be added "relative" w.r.t. the ``BIN_DIRS``
-#  - ``DEPENDS`` boolean variable. If ``TRUE`` RPATH will be enabled.
+#    directories will be added "relative" w.r.t. the ``BIN_DIRS`` and
+#    ``LIB_DIRS``.
+#  - ``INSTALL_NAME_DIR`` directory where the libraries will be installed.
+#    This variable will be used only if ``CMAKE_SKIP_RPATH`` or
+#    ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the
+#    ``INSTALL_NAME_DIR`` on all targets
+#  - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable
+#    RPATH, for example ``FOO; NOT BAR``.
+#
+# Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further
+# details.
 
 #=======================================================================
-# Copyright 2014 RBCS, Istituto Italiano di Tecnologia
+# Copyright 2014 Istituto Italiano di Tecnologia (IIT)
 # @author Francesco Romano <francesco.romano@iit.it>
 #
 # Distributed under the OSI-approved BSD License (the "License");
@@ -80,22 +92,46 @@
 include(CMakeParseArguments)
 
 
-macro(ADD_INSTALL_RPATH_SUPPORT)
+function(ADD_INSTALL_RPATH_SUPPORT)
+
+  set(_options USE_LINK_PATH)
+  set(_oneValueArgs INSTALL_NAME_DIR)
+  set(_multiValueArgs BIN_DIRS
+                      LIB_DIRS
+                      DEPENDS)
+
+  cmake_parse_arguments(_ARS "${_options}"
+                             "${_oneValueArgs}"
+                             "${_multiValueArgs}"
+                             "${ARGN}")
+
+  # if either RPATH or INSTALL_RPATH is disabled
+  # and the INSTALL_NAME_DIR variable is set, then hardcode the install name
+  if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH)
+    if(DEFINED _ARS_INSTALL_NAME_DIR)
+      set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE)
+    endif()
+  endif()
+
+  if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH))
+    return()
+  endif()
 
-set(_options USE_LINK_PATH)
-set(_oneValueArgs DEPENDS)
-set(_multiValueArgs BIN_DIRS
-                    LIB_DIRS)
 
-cmake_parse_arguments(_ARS "${_options}"
-                           "${_oneValueArgs}"
-                           "${_multiValueArgs}"
-                           "${ARGN}")
+  set(_rpath_available 1)
+  if(DEFINED _ARS_DEPENDS)
+    foreach(_dep ${_ARS_DEPENDS})
+      string(REGEX REPLACE " +" ";" _dep "${_dep}")
+      if(NOT (${_dep}))
+        set(_rpath_available 0)
+      endif()
+    endforeach()
+  endif()
 
-if(NOT DEFINED _ARS_DEPENDS OR _ARS_DEPENDS)
+  if(_rpath_available)
 
-    # Enable RPATH on OSX. This also suppress warnings on CMake >= 3.0
-    set(CMAKE_MACOSX_RPATH TRUE)
+    # Enable RPATH on OSX.
+    set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE)
 
     # Find system implicit lib directories
     set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES})
@@ -107,25 +143,27 @@ if(NOT DEFINED _ARS_DEPENDS OR _ARS_DEPENDS)
     endif()
     # This is relative RPATH for libraries built in the same project
     foreach(lib_dir ${_ARS_LIB_DIRS})
-        list(FIND _system_lib_dirs "${lib_dir}" isSystemDir)
-        if("${isSystemDir}" STREQUAL "-1")
-            foreach(bin_dir ${_ARS_BIN_DIRS})
-                file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir})
-                if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
-                    list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
-                else()
-                    list(APPEND CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}")
-                endif()
-            endforeach()
-        endif("${isSystemDir}" STREQUAL "-1")
+      list(FIND _system_lib_dirs "${lib_dir}" isSystemDir)
+      if("${isSystemDir}" STREQUAL "-1")
+        foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS})
+          file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir})
+          if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+            list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}")
+          else()
+            list(APPEND CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}")
+          endif()
+        endforeach()
+      endif()
     endforeach()
-
-    unset(_rel_path)
-    unset(_system_lib_dirs)
+    if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "")
+      list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH)
+    endif()
+    set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE)
 
     # add the automatically determined parts of the RPATH
     # which point to directories outside the build tree to the install RPATH
-    set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH})
-endif()
+    set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE)
+
+  endif()
 
-endmacro()
+endfunction()
diff --git a/cmake/AddUninstallTarget.cmake b/cmake/AddUninstallTarget.cmake
index cfec3f5a7c3..8c5da6d79a1 100644
--- a/cmake/AddUninstallTarget.cmake
+++ b/cmake/AddUninstallTarget.cmake
@@ -7,13 +7,24 @@
 #   include(AddUninstallTarget)
 #
 #
-# will create a file cmake_uninstall.cmake in the build directory and add a
-# custom target uninstall that will remove the files installed by your package
-# (using install_manifest.txt)
+# will create a file ``cmake_uninstall.cmake`` in the build directory and add a
+# custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that
+# will remove the files installed by your package (using
+# ``install_manifest.txt``).
+# See also
+# https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake
+#
+# The :module:`AddUninstallTarget` module must be included in your main
+# ``CMakeLists.txt``. If included in a subdirectory it does nothing.
+# This allows you to use it safely in your main ``CMakeLists.txt`` and include
+# your project using ``add_subdirectory`` (for example when using it with
+# :cmake:module:`FetchContent`).
+#
+# If the ``uninstall`` target already exists, the module does nothing.
 
 #=============================================================================
 # Copyright 2008-2013 Kitware, Inc.
-# Copyright 2013 iCub Facility, Istituto Italiano di Tecnologia
+# Copyright 2013 Istituto Italiano di Tecnologia (IIT)
 #   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
 #
 # Distributed under the OSI-approved BSD License (the "License");
@@ -27,18 +38,31 @@
 #  License text for the above reference.)
 
 
-if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED)
+# AddUninstallTarget works only when included in the main CMakeLists.txt
+if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}")
   return()
 endif()
-set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE)
 
+# The name of the target is uppercase in MSVC and Xcode (for coherence with the
+# other standard targets)
+if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)")
+  set(_uninstall "UNINSTALL")
+else()
+  set(_uninstall "uninstall")
+endif()
 
-set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake)
+# If target is already defined don't do anything
+if(TARGET ${_uninstall})
+  return()
+endif()
 
-file(WRITE ${_filename}
+
+set(_filename cmake_uninstall.cmake)
+
+file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}"
 "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\")
-    message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\")
-    return()
+  message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\")
+  return()
 endif()
 
 file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files)
@@ -46,25 +70,34 @@ string(STRIP \"\${files}\" files)
 string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\")
 list(REVERSE files)
 foreach(file \${files})
+  if(IS_SYMLINK \"\$ENV{DESTDIR}\${file}\" OR EXISTS \"\$ENV{DESTDIR}\${file}\")
     message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\")
-    if(EXISTS \"\$ENV{DESTDIR}\${file}\")
-        execute_process(
-            COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\"
-            OUTPUT_VARIABLE rm_out
-            RESULT_VARIABLE rm_retval)
-        if(NOT \"\${rm_retval}\" EQUAL 0)
-            message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\")
-        endif()
-    else()
-        message(STATUS \"File \\\"\$ENV{DESTDIR}\${file}\\\" does not exist.\")
+    execute_process(
+      COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\"
+      OUTPUT_VARIABLE rm_out
+      RESULT_VARIABLE rm_retval)
+    if(NOT \"\${rm_retval}\" EQUAL 0)
+      message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\")
     endif()
+  else()
+    message(STATUS \"Not-found: \$ENV{DESTDIR}\${file}\")
+  endif()
 endforeach(file)
 ")
 
-if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)")
-    set(_uninstall "UNINSTALL")
+set(_desc "Uninstall the project...")
+if(CMAKE_GENERATOR STREQUAL "Unix Makefiles")
+  set(_comment COMMAND \$\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}")
 else()
-    set(_uninstall "uninstall")
+  set(_comment COMMENT "${_desc}")
 endif()
-add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}")
+add_custom_target(${_uninstall}
+                  ${_comment}
+                  COMMAND ${CMAKE_COMMAND} -P ${_filename}
+                  USES_TERMINAL
+                  BYPRODUCTS uninstall_byproduct
+                  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}")
+set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1)
+
 set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets")
+
diff --git a/cmake/CMakePackageConfigHelpers.cmake b/cmake/CMakePackageConfigHelpers.cmake
deleted file mode 100644
index c6dc14199eb..00000000000
--- a/cmake/CMakePackageConfigHelpers.cmake
+++ /dev/null
@@ -1,321 +0,0 @@
-#.rst:
-# CMakePackageConfigHelpers
-# -------------------------
-#
-# Helpers functions for creating config files that can be included by other
-# projects to find and use a package.
-#
-# Adds the :command:`configure_package_config_file()` and
-# :command:`write_basic_package_version_file()` commands.
-#
-# Generating a Package Configuration File
-# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-#
-# .. command:: configure_package_config_file
-#
-#  Create a config file for a project::
-#
-#     configure_package_config_file(<input> <output> INSTALL_DESTINATION <path>
-#                                                    [PATH_VARS <var1> <var2> ... <varN>]
-#                                                    [NO_SET_AND_CHECK_MACRO]
-#                                                    [NO_CHECK_REQUIRED_COMPONENTS_MACRO]
-#                                                    [INSTALL_PREFIX <path>])
-#
-#
-# ``configure_package_config_file()`` should be used instead of the plain
-# :command:`configure_file()` command when creating the ``<Name>Config.cmake``
-# or ``<Name>-config.cmake`` file for installing a project or library.  It helps
-# making the resulting package relocatable by avoiding hardcoded paths in the
-# installed ``Config.cmake`` file.
-#
-# In a ``FooConfig.cmake`` file there may be code like this to make the install
-# destinations know to the using project:
-#
-# .. code-block:: cmake
-#
-#    set(FOO_INCLUDE_DIR   "@CMAKE_INSTALL_FULL_INCLUDEDIR@" )
-#    set(FOO_DATA_DIR   "@CMAKE_INSTALL_PREFIX@/@RELATIVE_DATA_INSTALL_DIR@" )
-#    set(FOO_ICONS_DIR   "@CMAKE_INSTALL_PREFIX@/share/icons" )
-#    ...logic to determine installedPrefix from the own location...
-#    set(FOO_CONFIG_DIR  "${installedPrefix}/@CONFIG_INSTALL_DIR@" )
-#
-# All 4 options shown above are not sufficient, since the first 3 hardcode the
-# absolute directory locations, and the 4th case works only if the logic to
-# determine the ``installedPrefix`` is correct, and if ``CONFIG_INSTALL_DIR``
-# contains a relative path, which in general cannot be guaranteed.  This has the
-# effect that the resulting ``FooConfig.cmake`` file would work poorly under
-# Windows and OSX, where users are used to choose the install location of a
-# binary package at install time, independent from how
-# :variable:`CMAKE_INSTALL_PREFIX` was set at build/cmake time.
-#
-# Using ``configure_package_config_file`` helps.  If used correctly, it makes
-# the resulting ``FooConfig.cmake`` file relocatable.  Usage:
-#
-#    1. write a ``FooConfig.cmake.in`` file as you are used to
-#    2. insert a line containing only the string ``@PACKAGE_INIT@``
-#    3. instead of ``set(FOO_DIR "@SOME_INSTALL_DIR@")``, use
-#       ``set(FOO_DIR "@PACKAGE_SOME_INSTALL_DIR@")`` (this must be after the
-#       ``@PACKAGE_INIT@`` line)
-#    4. instead of using the normal :command:`configure_file()`, use
-#       ``configure_package_config_file()``
-#
-#
-#
-# The ``<input>`` and ``<output>`` arguments are the input and output file, the
-# same way as in :command:`configure_file()`.
-#
-# The ``<path>`` given to ``INSTALL_DESTINATION`` must be the destination where
-# the ``FooConfig.cmake`` file will be installed to.  This path can either be
-# absolute, or relative to the ``INSTALL_PREFIX`` path.
-#
-# The variables ``<var1>`` to ``<varN>`` given as ``PATH_VARS`` are the
-# variables which contain install destinations.  For each of them the macro will
-# create a helper variable ``PACKAGE_<var...>``.  These helper variables must be
-# used in the ``FooConfig.cmake.in`` file for setting the installed location.
-# They are calculated by ``configure_package_config_file`` so that they are
-# always relative to the installed location of the package.  This works both for
-# relative and also for absolute locations.  For absolute locations it works
-# only if the absolute location is a subdirectory of ``INSTALL_PREFIX``.
-#
-# If the ``INSTALL_PREFIX`` argument is passed, this is used as base path to
-# calculate all the relative paths.  The ``<path>`` argument must be an absolute
-# path.  If this argument is not passed, the :variable:`CMAKE_INSTALL_PREFIX`
-# variable will be used instead.  The default value is good when generating a
-# FooConfig.cmake file to use your package from the install tree.  When
-# generating a FooConfig.cmake file to use your package from the build tree this
-# option should be used.
-#
-# By default ``configure_package_config_file`` also generates two helper macros,
-# ``set_and_check()`` and ``check_required_components()`` into the
-# ``FooConfig.cmake`` file.
-#
-# ``set_and_check()`` should be used instead of the normal ``set()`` command for
-# setting directories and file locations.  Additionally to setting the variable
-# it also checks that the referenced file or directory actually exists and fails
-# with a ``FATAL_ERROR`` otherwise.  This makes sure that the created
-# ``FooConfig.cmake`` file does not contain wrong references.
-# When using the ``NO_SET_AND_CHECK_MACRO``, this macro is not generated
-# into the ``FooConfig.cmake`` file.
-#
-# ``check_required_components(<package_name>)`` should be called at the end of
-# the ``FooConfig.cmake`` file if the package supports components.  This macro
-# checks whether all requested, non-optional components have been found, and if
-# this is not the case, sets the ``Foo_FOUND`` variable to ``FALSE``, so that
-# the package is considered to be not found.  It does that by testing the
-# ``Foo_<Component>_FOUND`` variables for all requested required components.
-# When using the ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` option, this macro is
-# not generated into the ``FooConfig.cmake`` file.
-#
-# For an example see below the documentation for
-# :command:`write_basic_package_version_file()`.
-#
-# Generating a Package Version File
-# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-#
-# .. command:: write_basic_package_version_file
-#
-#  Create a version file for a project::
-#
-#     write_basic_package_version_file(<filename>
-#                                      [VERSION <major.minor.patch>]
-#                                      COMPATIBILITY <AnyNewerVersion|SameMajorVersion|ExactVersion> )
-#
-#
-# Writes a file for use as ``<package>ConfigVersion.cmake`` file to
-# ``<filename>``.  See the documentation of :command:`find_package()` for
-# details on this.
-#
-# ``<filename>`` is the output filename, it should be in the build tree.
-# ``<major.minor.patch>`` is the version number of the project to be installed.
-#
-# If no ``VERSION`` is given, the :variable:`PROJECT_VERSION` variable is used.
-# If this hasn't been set, it errors out.
-#
-# The ``COMPATIBILITY`` mode ``AnyNewerVersion`` means that the installed
-# package version will be considered compatible if it is newer or exactly the
-# same as the requested version.  This mode should be used for packages which
-# are fully backward compatible, also across major versions.
-# If ``SameMajorVersion`` is used instead, then the behaviour differs from
-# ``AnyNewerVersion`` in that the major version number must be the same as
-# requested, e.g.  version 2.0 will not be considered compatible if 1.0 is
-# requested.  This mode should be used for packages which guarantee backward
-# compatibility within the same major version.
-# If ``ExactVersion`` is used, then the package is only considered compatible if
-# the requested version matches exactly its own version number (not considering
-# the tweak version).  For example, version 1.2.3 of a package is only
-# considered compatible to requested version 1.2.3.  This mode is for packages
-# without compatibility guarantees.
-# If your project has more elaborated version matching rules, you will need to
-# write your own custom ``ConfigVersion.cmake`` file instead of using this
-# macro.
-#
-# Internally, this macro executes :command:`configure_file()` to create the
-# resulting version file.  Depending on the ``COMPATIBLITY``, either the file
-# ``BasicConfigVersion-SameMajorVersion.cmake.in`` or
-# ``BasicConfigVersion-AnyNewerVersion.cmake.in`` is used.  Please note that
-# these two files are internal to CMake and you should not call
-# :command:`configure_file()` on them yourself, but they can be used as starting
-# point to create more sophisticted custom ``ConfigVersion.cmake`` files.
-#
-# Example Generating Package Files
-# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-#
-# Example using both :command:`configure_package_config_file` and
-# ``write_basic_package_version_file()``:
-#
-# ``CMakeLists.txt``:
-#
-# .. code-block:: cmake
-#
-#    set(INCLUDE_INSTALL_DIR include/ ... CACHE )
-#    set(LIB_INSTALL_DIR lib/ ... CACHE )
-#    set(SYSCONFIG_INSTALL_DIR etc/foo/ ... CACHE )
-#    ...
-#    include(CMakePackageConfigHelpers)
-#    configure_package_config_file(FooConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/FooConfig.cmake
-#                                  INSTALL_DESTINATION ${LIB_INSTALL_DIR}/Foo/cmake
-#                                  PATH_VARS INCLUDE_INSTALL_DIR SYSCONFIG_INSTALL_DIR)
-#    write_basic_package_version_file(${CMAKE_CURRENT_BINARY_DIR}/FooConfigVersion.cmake
-#                                     VERSION 1.2.3
-#                                     COMPATIBILITY SameMajorVersion )
-#    install(FILES ${CMAKE_CURRENT_BINARY_DIR}/FooConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/FooConfigVersion.cmake
-#            DESTINATION ${LIB_INSTALL_DIR}/Foo/cmake )
-#
-# ``FooConfig.cmake.in``:
-#
-# .. code-block:: cmake
-#
-#    set(FOO_VERSION x.y.z)
-#    ...
-#    @PACKAGE_INIT@
-#    ...
-#    set_and_check(FOO_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@")
-#    set_and_check(FOO_SYSCONFIG_DIR "@PACKAGE_SYSCONFIG_INSTALL_DIR@")
-#
-#    check_required_components(Foo)
-
-
-#=============================================================================
-# Copyright 2012 Alexander Neundorf <neundorf@kde.org>
-#
-# Distributed under the OSI-approved BSD License (the "License");
-# see accompanying file Copyright.txt for details.
-#
-# This software is distributed WITHOUT ANY WARRANTY; without even the
-# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-# See the License for more information.
-#=============================================================================
-# (To distribute this file outside of CMake, substitute the full
-#  License text for the above reference.)
-
-include(CMakeParseArguments)
-
-include(WriteBasicConfigVersionFile)
-
-macro(WRITE_BASIC_PACKAGE_VERSION_FILE)
-  write_basic_config_version_file(${ARGN})
-endmacro()
-
-function(CONFIGURE_PACKAGE_CONFIG_FILE _inputFile _outputFile)
-  set(options NO_SET_AND_CHECK_MACRO NO_CHECK_REQUIRED_COMPONENTS_MACRO)
-  set(oneValueArgs INSTALL_DESTINATION INSTALL_PREFIX)
-  set(multiValueArgs PATH_VARS )
-
-  cmake_parse_arguments(CCF "${options}" "${oneValueArgs}" "${multiValueArgs}"  ${ARGN})
-
-  if(CCF_UNPARSED_ARGUMENTS)
-    message(FATAL_ERROR "Unknown keywords given to CONFIGURE_PACKAGE_CONFIG_FILE(): \"${CCF_UNPARSED_ARGUMENTS}\"")
-  endif()
-
-  if(NOT CCF_INSTALL_DESTINATION)
-    message(FATAL_ERROR "No INSTALL_DESTINATION given to CONFIGURE_PACKAGE_CONFIG_FILE()")
-  endif()
-
-  if(DEFINED CCF_INSTALL_PREFIX)
-    if(IS_ABSOLUTE "${CCF_INSTALL_PREFIX}")
-      set(installPrefix "${CCF_INSTALL_PREFIX}")
-    else()
-      message(FATAL_ERROR "INSTALL_PREFIX must be an absolute path")
-    endif()
-  else()
-    set(installPrefix "${CMAKE_INSTALL_PREFIX}")
-  endif()
-
-  if(IS_ABSOLUTE "${CCF_INSTALL_DESTINATION}")
-    set(absInstallDir "${CCF_INSTALL_DESTINATION}")
-  else()
-    set(absInstallDir "${installPrefix}/${CCF_INSTALL_DESTINATION}")
-  endif()
-
-  file(RELATIVE_PATH PACKAGE_RELATIVE_PATH "${absInstallDir}" "${installPrefix}" )
-
-  foreach(var ${CCF_PATH_VARS})
-    if(NOT DEFINED ${var})
-      message(FATAL_ERROR "Variable ${var} does not exist")
-    else()
-      if(IS_ABSOLUTE "${${var}}")
-        string(REPLACE "${installPrefix}" "\${PACKAGE_PREFIX_DIR}"
-                        PACKAGE_${var} "${${var}}")
-      else()
-        set(PACKAGE_${var} "\${PACKAGE_PREFIX_DIR}/${${var}}")
-      endif()
-    endif()
-  endforeach()
-
-  get_filename_component(inputFileName "${_inputFile}" NAME)
-
-  set(PACKAGE_INIT "
-####### Expanded from @PACKAGE_INIT@ by configure_package_config_file() #######
-####### Any changes to this file will be overwritten by the next CMake run ####
-####### The input file was ${inputFileName}                            ########
-
-get_filename_component(PACKAGE_PREFIX_DIR \"\${CMAKE_CURRENT_LIST_DIR}/${PACKAGE_RELATIVE_PATH}\" ABSOLUTE)
-")
-
-  if("${absInstallDir}" MATCHES "^(/usr)?/lib(64)?/.+")
-    # Handle "/usr move" symlinks created by some Linux distros.
-    set(PACKAGE_INIT "${PACKAGE_INIT}
-# Use original install prefix when loaded through a \"/usr move\"
-# cross-prefix symbolic link such as /lib -> /usr/lib.
-get_filename_component(_realCurr \"\${CMAKE_CURRENT_LIST_DIR}\" REALPATH)
-get_filename_component(_realOrig \"${absInstallDir}\" REALPATH)
-if(_realCurr STREQUAL _realOrig)
-  set(PACKAGE_PREFIX_DIR \"${installPrefix}\")
-endif()
-unset(_realOrig)
-unset(_realCurr)
-")
-  endif()
-
-  if(NOT CCF_NO_SET_AND_CHECK_MACRO)
-    set(PACKAGE_INIT "${PACKAGE_INIT}
-macro(set_and_check _var _file)
-  set(\${_var} \"\${_file}\")
-  if(NOT EXISTS \"\${_file}\")
-    message(FATAL_ERROR \"File or directory \${_file} referenced by variable \${_var} does not exist !\")
-  endif()
-endmacro()
-")
-  endif()
-
-
-  if(NOT CCF_NO_CHECK_REQUIRED_COMPONENTS_MACRO)
-    set(PACKAGE_INIT "${PACKAGE_INIT}
-macro(check_required_components _NAME)
-  foreach(comp \${\${_NAME}_FIND_COMPONENTS})
-    if(NOT \${_NAME}_\${comp}_FOUND)
-      if(\${_NAME}_FIND_REQUIRED_\${comp})
-        set(\${_NAME}_FOUND FALSE)
-      endif()
-    endif()
-  endforeach()
-endmacro()
-")
-  endif()
-
-  set(PACKAGE_INIT "${PACKAGE_INIT}
-####################################################################################")
-
-  configure_file("${_inputFile}" "${_outputFile}" @ONLY)
-
-endfunction()
diff --git a/cmake/InstallBasicPackageFiles.cmake b/cmake/InstallBasicPackageFiles.cmake
index 2a2a35c3096..d00e80d9b03 100644
--- a/cmake/InstallBasicPackageFiles.cmake
+++ b/cmake/InstallBasicPackageFiles.cmake
@@ -12,56 +12,132 @@
 # project::
 #
 #  install_basic_package_files(<Name>
-#                              VERSION <version>
 #                              COMPATIBILITY <compatibility>
-#                              TARGETS_PROPERTY <property_name>
+#                              [VERSION <version>]
+#                              [ARCH_INDEPENDENT]
+#                              [NO_EXPORT | EXPORT <export>] # (default = "EXPORT <Name>")
 #                              [NO_SET_AND_CHECK_MACRO]
 #                              [NO_CHECK_REQUIRED_COMPONENTS_MACRO]
-#                              [VARS_PREFIX <prefix>] # (default = "<name>")
-#                              [DESTINATION <destination>]
-#                              [NAMESPACE <namespace>] # (default = "<name>::")
+#                              [VARS_PREFIX <prefix>] # (default = "<Name>")
+#                              [EXPORT_DESTINATION <destination>]
+#                              [INSTALL_DESTINATION <destination>]
+#                              [NAMESPACE <namespace>] # (default = "<Name>::")
 #                              [EXTRA_PATH_VARS_SUFFIX path1 [path2 ...]]
+#                              [CONFIG_TEMPLATE <file>]
 #                              [UPPERCASE_FILENAMES | LOWERCASE_FILENAMES]
+#                              [DEPENDENCIES <dependency1> "<dependency2> [...]" ...]
+#                              [PRIVATE_DEPENDENCIES <dependency1> "<dependency2> [...]" ...]
+#                              [INCLUDE_FILE <file> | INCLUDE_CONTENT <content>]
+#                              [COMPONENT <component>] # (default = "<Name>")
 #                             )
 #
-# Depending on UPPERCASE_FILENAMES and LOWERCASE_FILENAMES, this
+# Depending on ``UPPERCASE_FILENAMES`` and ``LOWERCASE_FILENAMES``, this
 # function generates 3 files:
 #
 #  - ``<Name>ConfigVersion.cmake`` or ``<name>-config-version.cmake``
 #  - ``<Name>Config.cmake`` or ``<name>-config.cmake``
 #  - ``<Name>Targets.cmake`` or ``<name>-targets.cmake``
 #
-# If neither UPPERCASE_FILENAMES nor LOWERCASE_FILENAMES is set, a file
-# ``<Name>ConfigVersion.cmake.in`` or
-# ``<name>-config-version.cmake.in`` is searched, and the convention
+# If neither ``UPPERCASE_FILENAMES`` nor ``LOWERCASE_FILENAMES`` is
+# set, a file ``<Name>Config.cmake.in`` or
+# ``<name>-config.cmake.in`` is searched, and the convention
 # is chosed according to the file found. If no file was found, the
 # uppercase convention is used.
 #
+# The ``DEPENDENCIES`` argument can be used to set a list of dependencies
+# that will be searched using the :cmake:command:`find_dependency` command
+# from the :module:`CMakeFindDependencyMacro` module.
+# Dependencies can be followed by any of the possible
+# :cmake:command:`find_dependency` argument.
+# In this case, all the arguments must be specified within double quotes (e.g.
+# ``"<dependency> 1.0.0 EXACT"``, or ``"<dependency> CONFIG"``).
+# The ``PRIVATE_DEPENDENCIES`` argument is similar to ``DEPENDENCIES``, but
+# these dependencies are included only when :variable:`BUILD_SHARED_LIBS` is
+# ``OFF``.
+# If a libraries is declared ``STATIC``, ``OBJECT`` or ``INTERFACE``, and they
+# link to some dependency, these should be added using the ``DEPENDENCIES``
+# argument, since the ``PRIVATE_DEPENDENCIES`` argument would work only when
+# :variable:`BUILD_SHARED_LIBS` is disabled.
+#
+# When using a custom template file, the ``@PACKAGE_DEPENDENCIES@``
+# string is replaced with the code checking for the dependencies
+# specified by these two argument.
+#
+# If the ``ARCH_INDEPENDENT`` option is enabled, the installed package version
+# will be considered compatible even if it was built for a different
+# architecture than the requested architecture.
+#
 # Each file is generated twice, one for the build directory and one for
-# the installation directory.  The ``DESTINATION`` argument can be
+# the installation directory.  The ``INSTALL_DESTINATION`` argument can be
 # passed to install the files in a location different from the default
-# one (``CMake`` on Windows, ``${CMAKE_INSTALL_LIBDIR}/cmake/${name}``
-# on other platforms.
+# one (``${CMAKE_INSTALL_DATADIR}/cmake/${Name}`` if the ``ARCH_INDEPENDENT``
+# option is enabled, ``${CMAKE_INSTALL_LIBDIR}/cmake/${Name}`` otherwise).
+# The ``EXPORT_DESTINATION`` argument can be passed to
+# generate the files in the build tree in a location different from the default
+# one (``CMAKE_BINARY_DIR``).  If this is a relative path, it is
+# considered relative to the ``CMAKE_CURRENT_BINARY_DIR`` directory.
+#
+# The build directory is exported to the CMake user package registry if the
+# build option ``CMAKE_EXPORT_PACKAGE_REGISTRY`` is set.
+#
+# The ``<Name>ConfigVersion.cmake`` file is generated using
+# :cmake:command:`write_basic_package_version_file`. The ``VERSION``,
+# ``COMPATIBILITY``, and ``ARCH_INDEPENDENT``arguments are passed to this
+# function.
+#
+# ``VERSION`` shall be in the form ``<major>[.<minor>[.<patch>[.<tweak>]]]]``.
+# If no ``VERSION`` is given, the ``PROJECT_VERSION`` variable is used.
+# If this hasn’t been set, it errors out.  The ``VERSION`` argument is also used
+# to replace the ``@PACKAGE_VERSION@`` string in the configuration file.
+#
+# ``COMPATIBILITY`` shall be any of the options accepted by the
+# :cmake:command:`write_basic_package_version_file` command
+# (``AnyNewerVersion``, ``SameMajorVersion``, ``SameMinorVersion`` [CMake 3.11],
+# or ``ExactVersion``).
+# These options are explained in :cmake:command:`write_basic_package_version_file`
+# command documentation.
+# If your project has more elaborate version matching rules, you will need to
+# write your own custom ConfigVersion.cmake file instead of using this macro.
+#
+# The ``<Name>Config.cmake`` file is generated using
+# :cmake:command:`configure_package_config_file`. The
+# ``NO_SET_AND_CHECK_MACRO``, ``NO_CHECK_REQUIRED_COMPONENTS_MACRO``, and
+# arguments are passed to this function.
 #
+# By default :command:`install_basic_package_files` also generates the two helper
+# macros ``set_and_check()`` and ``check_required_components()`` into the
+# ``<Name>Config.cmake`` file. ``set_and_check()`` should be used instead of the
+# normal :cmake:command:`set()` command for setting directories and file locations.
+# Additionally to setting the variable it also checks that the referenced file
+# or directory actually exists and fails with a ``FATAL_ERROR`` otherwise.
+# This makes sure that the created ``<Name>Config.cmake`` file does not contain
+# wrong references.
+# When using the ``NO_SET_AND_CHECK_MACRO, this macro is not generated into the
+# ``<Name>Config.cmake`` file.
 #
-# The ``<Name>ConfigVersion.cmake`` is generated using
-# ``write_basic_package_version_file``.  The ``VERSION``,
-# ``COMPATIBILITY``, ``NO_SET_AND_CHECK_MACRO``, and
-# ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` are passed to this function.
-# See the documentation for the :module:`CMakePackageConfigHelpers`
-# module for further information. The files in the build and install
-# directory are exactly the same.
+# By default, :command:`install_basic_package_files` append a call to
+# ``check_required_components(<Name>)`` in ``<Name>Config.cmake`` file if the
+# package supports components. This macro checks whether all requested,
+# non-optional components have been found, and if this is not the case, sets the
+# ``<Name>_FOUND`` variable to ``FALSE``, so that the package is considered to
+# be not found. It does that by testing the ``<Name>_<Component>_FOUND``
+# variables for all requested required components. When using the
+# ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` option, this macro is not generated
+# into the ``<Name>Config.cmake`` file.
 #
+# Finally, the files in the build and install directory are exactly the same.
 #
-# The ``<Name>Config.cmake`` is generated using
-# ``configure_package_config_file``.  See the documentation for the
-# :module:`CMakePackageConfigHelpers` module for further information.
-# The module expects to find a ``<Name>Config.cmake.in`` or
-# ``<name>-config.cmake.in`` file in the root directory of the project.
+# See the documentation of :module:`CMakePackageConfigHelpers` module for
+# further information and references therein.
+#
+# If the ``CONFIG_TEMPLATE`` argument is passed, the specified file
+# is used as template for generating the configuration file, otherwise
+# this module expects to find a ``<Name>Config.cmake.in`` or
+# ``<name>-config.cmake.in`` file either in current source directory.
 # If the file does not exist, a very basic file is created.
 #
 # A set of variables are checked and passed to
-# ``configure_package_config_file`` as ``PATH_VARS``. For each of the
+# :cmake:command:`configure_package_config_file` as ``PATH_VARS``. For each of the
 # ``SUFFIX`` considered, if one of the variables::
 #
 #     <VARS_PREFIX>_(BUILD|INSTALL)_<SUFFIX>
@@ -69,11 +145,16 @@
 #
 # is defined, the ``<VARS_PREFIX>_<SUFFIX>`` variable will be defined
 # before configuring the package.  In order to use that variable in the
-# config file, a line ou can access to that variable in the config
-# file by using::
+# config file, you have to add a line::
 #
 #   set_and_check(<VARS_PREFIX>_<SUFFIX> \"@PACKAGE_<VARS_PREFIX>_<SUFFIX>@\")
 #
+# if the path must exist or just::
+#
+#   set(<VARS_PREFIX>_<SUFFIX> \"@PACKAGE_<VARS_PREFIX>_<SUFFIX>@\")
+#
+# if the path could be missing.
+#
 # These variable will have different values whether you are using the
 # package from the build tree or from the install directory.  Also these
 # files will contain only relative paths, meaning that you can move the
@@ -101,16 +182,34 @@
 # argument.
 #
 #
-# The ``<name>Targets.cmake`` is generated using
-# :command:`export(TARGETS)` in the build tree and
-# :command:`install(EXPORT)` in the installation directory.
-# The targets are exported using the value for the ``NAMESPACE``
+# The ``<Name>Targets.cmake`` is generated using :cmake:command:`export(EXPORT)`
+# in the build tree and :cmake:command:`install(EXPORT)` in the installation
+# directory. The targets are exported using the value for the ``NAMESPACE``
 # argument as namespace.
-# The targets should be listed in a global property, that must be passed
-# to the function using the ``TARGETS_PROPERTY`` argument
+# The export can be passed using the ``EXPORT`` argument. If no export is
+# used (e.g. for a CMake script library), pass ``NO_EXPORT``.
+#
+# If the ``INCLUDE_FILE`` argument is passed, the content of the specified file
+# (which might contain ``@variables@``) is appended to the generated
+# ``<Name>Config.cmake`` file.
+# If the ``INCLUDE_CONTENT`` argument is passed, the specified content
+# (which might contain ``@variables@``) is appended to the generated
+# ``<Name>Config.cmake`` file.
+# When a ``CONFIG_TEMPLATE`` is passed, or a ``<Name>ConfigVersion.cmake.in`` or
+# a ``<name>-config-version.cmake.in file is available, these 2 arguments are
+# used to replace the ``@INCLUDED_CONTENT@`` string in this file.
+# This allows one to inject custom code to this file, useful e.g. to set
+# additional variables which are loaded by downstream projects.
+#
+# Note that content specified with ``INCLUDE_FILE`` or ``INCLUDE_CONTENT``
+# cannot reference any of the ``PATH_VARS`` because this content is not
+# expanded by :cmake:command:`configure_package_config_file`.
+#
+# If the ``COMPONENT`` argument is passed, it is forwarded to the
+# :cmake:command:`install` commands, otherwise ``<Name>`` is used.
 
 #=============================================================================
-# Copyright 2013 iCub Facility, Istituto Italiano di Tecnologia
+# Copyright 2013 Istituto Italiano di Tecnologia (IIT)
 #   Authors: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
 #
 # Distributed under the OSI-approved BSD License (the "License");
@@ -125,7 +224,7 @@
 
 
 if(COMMAND install_basic_package_files)
-    return()
+  return()
 endif()
 
 
@@ -136,203 +235,457 @@ include(CMakeParseArguments)
 
 function(INSTALL_BASIC_PACKAGE_FILES _Name)
 
-    # TODO check that _Name does not contain "-" characters
-
-    set(_options NO_SET_AND_CHECK_MACRO
-                 NO_CHECK_REQUIRED_COMPONENTS_MACRO
-                 UPPERCASE_FILENAMES
-                 LOWERCASE_FILENAMES)
-    set(_oneValueArgs VERSION
-                      COMPATIBILITY
-                      TARGETS_PROPERTY
-                      VARS_PREFIX
-                      DESTINATION
-                      NAMESPACE)
-    set(_multiValueArgs EXTRA_PATH_VARS_SUFFIX)
-    cmake_parse_arguments(_IBPF "${_options}" "${_oneValueArgs}" "${_multiValueArgs}" "${ARGN}")
-
-    if(NOT DEFINED _IBPF_VARS_PREFIX)
-        set(_IBPF_VARS_PREFIX ${_Name})
+  set(_options ARCH_INDEPENDENT
+               NO_EXPORT
+               NO_SET_AND_CHECK_MACRO
+               NO_CHECK_REQUIRED_COMPONENTS_MACRO
+               UPPERCASE_FILENAMES
+               LOWERCASE_FILENAMES
+               NO_COMPATIBILITY_VARS # Deprecated
+               ENABLE_COMPATIBILITY_VARS) # Deprecated
+  set(_oneValueArgs VERSION
+                    COMPATIBILITY
+                    EXPORT
+                    FIRST_TARGET # Deprecated
+                    TARGETS_PROPERTY # Deprecated
+                    VARS_PREFIX
+                    EXPORT_DESTINATION
+                    INSTALL_DESTINATION
+                    DESTINATION # Deprecated
+                    NAMESPACE
+                    CONFIG_TEMPLATE
+                    INCLUDE_FILE
+                    INCLUDE_CONTENT
+                    COMPONENT)
+  set(_multiValueArgs EXTRA_PATH_VARS_SUFFIX
+                      TARGETS # Deprecated
+                      TARGETS_PROPERTIES # Deprecated
+                      DEPENDENCIES
+                      PRIVATE_DEPENDENCIES)
+  cmake_parse_arguments(_IBPF "${_options}" "${_oneValueArgs}" "${_multiValueArgs}" "${ARGN}")
+
+  if(NOT DEFINED _IBPF_VARS_PREFIX)
+    set(_IBPF_VARS_PREFIX ${_Name})
+  endif()
+
+  if(NOT DEFINED _IBPF_VERSION)
+    if(NOT DEFINED PROJECT_VERSION)
+      message(FATAL_ERROR "VERSION argument is required (PROJECT_VERSION is not defined)")
     endif()
-
-    if(NOT DEFINED _IBPF_VERSION)
-        message(FATAL_ERROR "VERSION argument is required")
+    set(_IBPF_VERSION ${PROJECT_VERSION})
+  endif()
+
+  if(NOT DEFINED _IBPF_COMPATIBILITY)
+    message(FATAL_ERROR "COMPATIBILITY argument is required")
+  endif()
+
+  unset(_arch_independent)
+  if(_IBPF_ARCH_INDEPENDENT)
+    set(_arch_independent ARCH_INDEPENDENT)
+  endif()
+
+  if(_IBPF_UPPERCASE_FILENAMES AND _IBPF_LOWERCASE_FILENAMES)
+    message(FATAL_ERROR "UPPERCASE_FILENAMES and LOWERCASE_FILENAMES arguments cannot be used together")
+  endif()
+
+  if(DEFINED _IBPF_INCLUDE_FILE AND DEFINED _IBPF_INCLUDE_CONTENT)
+    message(FATAL_ERROR "INCLUDE_FILE and INCLUDE_CONTENT arguments cannot be used together")
+  endif()
+
+  # Prepare install and export commands
+  unset(_targets)
+  set(_install_cmd EXPORT ${_Name})
+  set(_export_cmd EXPORT ${_Name})
+
+  if(DEFINED _IBPF_EXPORT)
+    if(_IBPF_NO_EXPORT OR DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES)
+      message(FATAL_ERROR "EXPORT cannot be used with NO_EXPORT, TARGETS, TARGETS_PROPERTY, or TARGETS_PROPERTIES")
     endif()
 
-    if(NOT DEFINED _IBPF_COMPATIBILITY)
-        message(FATAL_ERROR "COMPATIBILITY argument is required")
-    endif()
+    set(_export_cmd EXPORT ${_IBPF_EXPORT})
+    set(_install_cmd EXPORT ${_IBPF_EXPORT})
 
-    if(NOT DEFINED _IBPF_TARGETS_PROPERTY)
-        message(FATAL_ERROR "TARGETS_PROPERTY argument is required")
+  elseif(_IBPF_NO_EXPORT)
+    if(DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES)
+      message(FATAL_ERROR "NO_EXPORT cannot be used with TARGETS, TARGETS_PROPERTY, or TARGETS_PROPERTIES")
     endif()
 
-    if(_IBPF_UPPERCASE_FILENAMES AND _IBPF_LOWERCASE_FILENAMES)
-        message(FATAL_ERROR "UPPERCASE_FILENAMES and LOWERCASE_FILENAMES arguments cannot be used together")
-    endif()
+  elseif(DEFINED _IBPF_TARGETS)
+    message(DEPRECATION "TARGETS is deprecated. Use EXPORT instead")
 
-    # Path for installed cmake files
-    if(NOT DEFINED _IBPF_DESTINATION)
-        if(WIN32 AND NOT CYGWIN)
-            set(_IBPF_DESTINATION CMake)
-        else()
-            set(_IBPF_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${_Name})
-        endif()
+    if(DEFINED _IBPF_TARGETS_PROPERTY OR DEFINED _IBPF_TARGETS_PROPERTIES)
+      message(FATAL_ERROR "TARGETS cannot be used with TARGETS_PROPERTY or TARGETS_PROPERTIES")
     endif()
 
-    if(NOT DEFINED _IBPF_NAMESPACE)
-        set(_IBPF_NAMESPACE "${_Name}::")
-    endif()
+    set(_targets ${_IBPF_TARGETS})
+    set(_export_cmd TARGETS ${_IBPF_TARGETS})
 
-    if(_IBPF_NO_SET_AND_CHECK_MACRO)
-        list(APPEND configure_package_config_file_extra_args NO_SET_AND_CHECK_MACRO)
-    endif()
+  elseif(DEFINED _IBPF_TARGETS_PROPERTY)
+    message(DEPRECATION "TARGETS_PROPERTY is deprecated. Use EXPORT instead")
 
-    if(_IBPF_NO_CHECK_REQUIRED_COMPONENTS_MACRO)
-        list(APPEND configure_package_config_file_extra_args NO_CHECK_REQUIRED_COMPONENTS_MACRO)
+    if(DEFINED _IBPF_TARGETS_PROPERTIES)
+      message(FATAL_ERROR "TARGETS_PROPERTIES cannot be used with TARGETS_PROPERTIES")
     endif()
 
-    string(TOLOWER "${_Name}" _name)
-    if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
-        if(EXISTS ${CMAKE_SOURCE_DIR}/${_Name}Config.cmake.in)
-            set(_IBPF_UPPERCASE_FILENAMES 1)
-        elseif(EXISTS${CMAKE_SOURCE_DIR}/${_name}-config.cmake.in)
-            set(_IBPF_LOWERCASE_FILENAMES 1)
-        else()
-            set(_IBPF_UPPERCASE_FILENAMES 1)
-        endif()
-    endif()
+    get_property(_targets GLOBAL PROPERTY ${_IBPF_TARGETS_PROPERTY})
+    set(_export_cmd TARGETS ${_targets})
 
-    if(_IBPF_UPPERCASE_FILENAMES)
-        set(_config_filename ${_Name}Config.cmake)
-        set(_version_filename ${_Name}ConfigVersion.cmake)
-        set(_targets_filename ${_Name}Targets.cmake)
-    elseif(_IBPF_LOWERCASE_FILENAMES)
-        set(_config_filename ${_name}-config.cmake)
-        set(_version_filename ${_name}-config-version.cmake)
-        set(_targets_filename ${_name}-targets.cmake)
-    endif()
+  elseif(DEFINED _IBPF_TARGETS_PROPERTIES)
+    message(DEPRECATION "TARGETS_PROPERTIES is deprecated. Use EXPORT instead")
 
-    # Make relative paths absolute (needed later on) and append the
-    # defined variables to _(build|install)_path_vars_suffix
-    foreach(p BINDIR          BIN_DIR
-              SBINDIR         SBIN_DIR
-              LIBEXECDIR      LIBEXEC_DIR
-              SYSCONFDIR      SYSCONF_DIR
-              SHAREDSTATEDIR  SHAREDSTATE_DIR
-              LOCALSTATEDIR   LOCALSTATE_DIR
-              LIBDIR          LIB_DIR
-              INCLUDEDIR      INCLUDE_DIR
-              OLDINCLUDEDIR   OLDINCLUDE_DIR
-              DATAROOTDIR     DATAROOT_DIR
-              DATADIR         DATA_DIR
-              INFODIR         INFO_DIR
-              LOCALEDIR       LOCALE_DIR
-              MANDIR          MAN_DIR
-              DOCDIR          DOC_DIR
-              ${_IBPF_EXTRA_PATH_VARS_SUFFIX})
-        if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p})
-            list(APPEND _build_path_vars_suffix ${p})
-            list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}")
-        endif()
-        if(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p})
-            list(APPEND _build_path_vars_suffix ${p})
-            list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}")
-        endif()
-        if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p})
-            list(APPEND _install_path_vars_suffix ${p})
-            list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}")
-        endif()
-        if(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p})
-            list(APPEND _install_path_vars_suffix ${p})
-            list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}")
-        endif()
+    set(_targets "") # Defined but empty
+    foreach(_prop ${_IBPF_TARGETS_PROPERTIES})
+      get_property(_prop_val GLOBAL PROPERTY ${_prop})
+      list(APPEND _targets ${_prop_val})
     endforeach()
+    set(_export_cmd TARGETS ${_targets})
 
+  endif()
 
+  # Path for installed cmake files
+  if(DEFINED _IBPF_DESTINATION)
+    message(DEPRECATION "DESTINATION is deprecated. Use INSTALL_DESTINATION instead")
+    if(NOT DEFINED _IBPF_INSTALL_DESTINATION)
+      set(_IBPF_INSTALL_DESTINATION ${_IBPF_DESTINATION})
+    endif()
+  endif()
+
+  # If not set by the user, choose an adequate destination
+  if(NOT DEFINED _IBPF_INSTALL_DESTINATION)
+    if(_IBPF_ARCH_INDEPENDENT)
+      set(_IBPF_INSTALL_DESTINATION ${CMAKE_INSTALL_DATADIR}/cmake/${_Name})
+    else()
+      set(_IBPF_INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${_Name})
+    endif()
+  endif()
+
+  # FIRST_TARGET is no longer used
+  if(DEFINED _IBPF_FIRST_TARGET)
+    message(DEPRECATION "FIRST_TARGET is deprecated.")
+  endif()
+
+  # NO_COMPATIBILITY_VARS and ENABLE_COMPATIBILITY_VARS cannot be used together
+  if(_IBPF_NO_COMPATIBILITY_VARS AND _ENABLE_COMPATIBILITY_VARS)
+    message(FATAL_ERROR "NO_COMPATIBILITY_VARS and ENABLE_COMPATIBILITY_VARS cannot be used together")
+  endif()
+  # NO_COMPATIBILITY_VARS is deprecated
+  if(_IBPF_NO_COMPATIBILITY_VARS)
+    message(DEPRECATION "NO_COMPATIBILITY_VARS is deprecated.")
+  endif()
+  # ENABLE_COMPATIBILITY_VARS is deprecated
+  if(_IBPF_ENABLE_COMPATIBILITY_VARS)
+    message(DEPRECATION "ENABLE_COMPATIBILITY_VARS is deprecated.")
+  endif()
+  # ENABLE_COMPATIBILITY_VARS does not work with EXPORT
+  if(NOT DEFINED _targets AND _IBPF_ENABLE_COMPATIBILITY_VARS)
+    message(FATAL_ERROR "ENABLE_COMPATIBILITY_VARS does not work with EXPORT")
+  endif()
+  # ENABLE_COMPATIBILITY_VARS can be enabled for projects still using targets
+  if(DEFINED _targets AND NOT _IBPF_NO_COMPATIBILITY_VARS AND NOT _IBPF_ENABLE_COMPATIBILITY_VARS)
+    message(DEPRECATION "Compatibility variables are no longer generated. Use ENABLE_COMPATIBILITY_VARS to re-enable them (deprecated) or define them using either INCLUDE_FILE or INCLUDE_CONTENT (recommended).")
+  endif()
+
+  if(NOT DEFINED _IBPF_EXPORT_DESTINATION)
+    set(_IBPF_EXPORT_DESTINATION "${CMAKE_BINARY_DIR}")
+  elseif(NOT IS_ABSOLUTE "${_IBPF_EXPORT_DESTINATION}")
+    set(_IBPF_EXPORT_DESTINATION "${CMAKE_CURRENT_BINARY_DIR}/${_IBPF_EXPORT_DESTINATION}")
+  endif()
+
+  if(NOT DEFINED _IBPF_NAMESPACE)
+    set(_IBPF_NAMESPACE "${_Name}::")
+  endif()
+
+  if(NOT DEFINED _IBPF_COMPONENT)
+    set(_IBPF_COMPONENT "${_Name}")
+  endif()
+
+  if(_IBPF_NO_SET_AND_CHECK_MACRO)
+    list(APPEND configure_package_config_file_extra_args NO_SET_AND_CHECK_MACRO)
+  endif()
+
+  if(_IBPF_NO_CHECK_REQUIRED_COMPONENTS_MACRO)
+    list(APPEND configure_package_config_file_extra_args NO_CHECK_REQUIRED_COMPONENTS_MACRO)
+  endif()
+
+
+
+  # Set input file for config, and ensure that _IBPF_UPPERCASE_FILENAMES
+  # and _IBPF_LOWERCASE_FILENAMES are set correctly
+  unset(_config_cmake_in)
+  set(_generate_file 0)
+  if(DEFINED _IBPF_CONFIG_TEMPLATE)
+    if(NOT EXISTS "${_IBPF_CONFIG_TEMPLATE}")
+      message(FATAL_ERROR "Config template file \"${_IBPF_CONFIG_TEMPLATE}\" not found")
+    endif()
+    set(_config_cmake_in "${_IBPF_CONFIG_TEMPLATE}")
+    if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
+      if("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_Name}Config.cmake.in")
+        set(_IBPF_UPPERCASE_FILENAMES 1)
+      elseif("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_name}-config.cmake.in")
+        set(_IBPF_LOWERCASE_FILENAMES 1)
+      else()
+        set(_IBPF_UPPERCASE_FILENAMES 1)
+      endif()
+    endif()
+  else()
+    string(TOLOWER "${_Name}" _name)
+    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in")
+      set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in")
+      if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
+        set(_IBPF_UPPERCASE_FILENAMES 1)
+      endif()
+    elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in")
+      set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in")
+      if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES)
+        set(_IBPF_LOWERCASE_FILENAMES 1)
+      endif()
+    else()
+      set(_generate_file 1)
+      if(_IBPF_LOWERCASE_FILENAMES)
+        set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_name}-config.cmake.in")
+      else()
+        set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_Name}Config.cmake.in")
+        set(_IBPF_UPPERCASE_FILENAMES 1)
+      endif()
+    endif()
+  endif()
+
+  # Set input file containing user variables
+  if(DEFINED _IBPF_INCLUDE_FILE)
+    if(NOT IS_ABSOLUTE "${_IBPF_INCLUDE_FILE}")
+      if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}")
+        set(_IBPF_INCLUDE_FILE "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}")
+      endif()
+    endif()
+    if(NOT EXISTS "${_IBPF_INCLUDE_FILE}")
+      message(FATAL_ERROR "File \"${_IBPF_INCLUDE_FILE}\" not found")
+    endif()
+    file(READ ${_IBPF_INCLUDE_FILE} _IBPF_INCLUDE_CONTENT)
+  endif()
 
-    # Get targets from GLOBAL PROPERTY
-    get_property(_targets GLOBAL PROPERTY ${_IBPF_TARGETS_PROPERTY})
-    foreach(_target ${_targets})
-        list(APPEND ${_IBPF_VARS_PREFIX}_TARGETS ${_Name}::${_target})
-    endforeach()
-    list(GET ${_IBPF_VARS_PREFIX}_TARGETS 0 _target)
-
+  if(DEFINED _IBPF_INCLUDE_CONTENT)
+    string(CONFIGURE ${_IBPF_INCLUDE_CONTENT}
+           _IBPF_INCLUDE_CONTENT
+           @ONLY)
+    set(INCLUDED_CONTENT
+"#### Expanded from INCLUDE_FILE/INCLUDE_CONTENT by install_basic_package_files() ####
 
+${_IBPF_INCLUDE_CONTENT}
 
-    # <name>ConfigVersion.cmake file (same for build tree and intall)
-    write_basic_package_version_file(${CMAKE_BINARY_DIR}/${_version_filename}
-                                    VERSION ${_IBPF_VERSION}
-                                    COMPATIBILITY ${_IBPF_COMPATIBILITY})
-    install(FILES ${CMAKE_BINARY_DIR}/${_version_filename}
-            DESTINATION ${_IBPF_DESTINATION})
+#####################################################################################
+")
+  endif()
+
+  # Backwards compatibility
+  if(NOT _generate_file AND DEFINED _IBPF_INCLUDE_FILE)
+    file(READ ${_config_cmake_in} _config_cmake_in_content)
+    if("${_config_cmake_in_content}" MATCHES "@INCLUDED_FILE_CONTENT@")
+      message(DEPRECATION "The @INCLUDED_FILE_CONTENT@ variable is deprecated in favour of @INCLUDED_CONTENT@")
+      set(INCLUDED_FILE_CONTENT "${INCLUDED_CONTENT}")
+    endif()
+  endif()
+
+  # Select output file names
+  if(_IBPF_UPPERCASE_FILENAMES)
+    set(_config_filename ${_Name}Config.cmake)
+    set(_version_filename ${_Name}ConfigVersion.cmake)
+    set(_targets_filename ${_Name}Targets.cmake)
+  elseif(_IBPF_LOWERCASE_FILENAMES)
+    set(_config_filename ${_name}-config.cmake)
+    set(_version_filename ${_name}-config-version.cmake)
+    set(_targets_filename ${_name}-targets.cmake)
+  endif()
+
+
+  # If the template config file does not exist, write a basic one
+  if(_generate_file)
+    # Generate the compatibility code
+    unset(_compatibility_vars)
+    if(_IBPF_ENABLE_COMPATIBILITY_VARS)
+      unset(_get_include_dir_code)
+      unset(_set_include_dir_code)
+      unset(_target_list)
+      foreach(_target ${_targets})
+        list(APPEND _target_list ${_IBPF_NAMESPACE}${_target})
+      endforeach()
+      if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDEDIR OR
+         DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDEDIR OR
+         DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDEDIR OR
+         DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDEDIR)
+        list(APPEND _include_dir_list "\"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDEDIR\@\"")
+      elseif(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDE_DIR OR
+             DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDE_DIR OR
+             DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDE_DIR OR
+             DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDE_DIR)
+        list(APPEND _include_dir_list "\"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDE_DIR\@\"")
+      else()
+        unset(_include_dir_list)
+        foreach(_target ${_targets})
+          list(APPEND _include_dir_list "\$<TARGET_PROPERTY:${_IBPF_NAMESPACE}${_target},INTERFACE_INCLUDE_DIRECTORIES>")
+        endforeach()
+        string(REPLACE ";" " " _include_dir_list "${_include_dir_list}")
+        string(REPLACE ";" " " _target_list "${_target_list}")
+        set(_set_include_dir "")
+      endif()
+      set(_compatibility_vars
+"# Compatibility\nset(${_Name}_LIBRARIES ${_target_list})
+set(${_Name}_INCLUDE_DIRS ${_include_dir_list})
+if(NOT \"\${${_Name}_INCLUDE_DIRS}\" STREQUAL \"\")
+  list(REMOVE_DUPLICATES ${_Name}_INCLUDE_DIRS)
+endif()
+")
+    endif()
 
+    if(_IBPF_NO_EXPORT)
+      set(_include_targets_cmd "")
+    else()
+      set(_include_targets_cmd "include(\"\${CMAKE_CURRENT_LIST_DIR}/${_targets_filename}\")")
+    endif()
 
+    # Write the file
+    file(WRITE "${_config_cmake_in}"
+"set(${_IBPF_VARS_PREFIX}_VERSION \@PACKAGE_VERSION\@)
 
-    # If there is no Config.cmake.in file, write a basic one
-    set(_config_cmake_in ${CMAKE_SOURCE_DIR}/${_config_filename}.in)
-    if(NOT EXISTS ${_config_cmake_in})
-        set(_config_cmake_in ${CMAKE_BINARY_DIR}/${_config_filename}.in)
-        file(WRITE ${_config_cmake_in}
-"set(${_IBPF_VARS_PREFIX}_VERSION \@${_IBPF_VARS_PREFIX}_VERSION\@)
+\@PACKAGE_INIT\@
 
-@PACKAGE_INIT@
+\@PACKAGE_DEPENDENCIES\@
 
-set_and_check(${_IBPF_VARS_PREFIX}_INCLUDEDIR \"@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDEDIR@\")
+${_include_targets_cmd}
 
-if(NOT TARGET ${_target})
-  include(\"\${CMAKE_CURRENT_LIST_DIR}/${_targets_filename}\")
-endif()
+${_compatibility_vars}
 
-# Compatibility
-set(${_Name}_LIBRARIES ${${_IBPF_VARS_PREFIX}_TARGETS})
-set(${_Name}_INCLUDE_DIRS \${${_IBPF_VARS_PREFIX}_INCLUDEDIR})
+\@INCLUDED_CONTENT\@
 ")
+  endif()
+
+  # Make relative paths absolute (needed later on) and append the
+  # defined variables to _(build|install)_path_vars_suffix
+  foreach(p BINDIR          BIN_DIR
+            SBINDIR         SBIN_DIR
+            LIBEXECDIR      LIBEXEC_DIR
+            SYSCONFDIR      SYSCONF_DIR
+            SHAREDSTATEDIR  SHAREDSTATE_DIR
+            LOCALSTATEDIR   LOCALSTATE_DIR
+            LIBDIR          LIB_DIR
+            INCLUDEDIR      INCLUDE_DIR
+            OLDINCLUDEDIR   OLDINCLUDE_DIR
+            DATAROOTDIR     DATAROOT_DIR
+            DATADIR         DATA_DIR
+            INFODIR         INFO_DIR
+            LOCALEDIR       LOCALE_DIR
+            MANDIR          MAN_DIR
+            DOCDIR          DOC_DIR
+            ${_IBPF_EXTRA_PATH_VARS_SUFFIX})
+    if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p})
+      list(APPEND _build_path_vars_suffix ${p})
+      list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}")
+    endif()
+    if(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p})
+      list(APPEND _build_path_vars_suffix ${p})
+      list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}")
     endif()
+    if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p})
+      list(APPEND _install_path_vars_suffix ${p})
+      list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}")
+    endif()
+    if(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p})
+      list(APPEND _install_path_vars_suffix ${p})
+      list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}")
+    endif()
+  endforeach()
 
-    # <name>Config.cmake (build tree)
-    foreach(p ${_build_path_vars_suffix})
-        if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p})
-            set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_BUILD_${p}}")
-        elseif(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p})
-            set(${_IBPF_VARS_PREFIX}_${p} "${BUILD_${_IBPF_VARS_PREFIX}_${p}}")
-        endif()
-    endforeach()
-    configure_package_config_file(${_config_cmake_in}
-                                  ${CMAKE_BINARY_DIR}/${_config_filename}
-                                  INSTALL_DESTINATION ${CMAKE_BINARY_DIR}
-                                  PATH_VARS ${_build_path_vars}
-                                  ${configure_package_config_file_extra_args}
-                                  INSTALL_PREFIX ${CMAKE_BINARY_DIR})
-
-    # <name>Config.cmake (installed)
-    foreach(p ${_install_path_vars_suffix})
-        if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p})
-            set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_INSTALL_${p}}")
-        elseif(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p})
-            set(${_IBPF_VARS_PREFIX}_${p} "${INSTALL_${_IBPF_VARS_PREFIX}_${p}}")
-        endif()
+
+  # <Name>ConfigVersion.cmake file (same for build tree and intall)
+  write_basic_package_version_file("${_IBPF_EXPORT_DESTINATION}/${_version_filename}"
+                                   VERSION ${_IBPF_VERSION}
+                                   COMPATIBILITY ${_IBPF_COMPATIBILITY}
+                                   ${_arch_independent})
+  install(FILES "${_IBPF_EXPORT_DESTINATION}/${_version_filename}"
+          DESTINATION ${_IBPF_INSTALL_DESTINATION}
+          COMPONENT ${_IBPF_COMPONENT})
+
+
+  # Prepare PACKAGE_DEPENDENCIES variable
+  set(_need_private_deps 0)
+  if(NOT BUILD_SHARED_LIBS)
+    set(_need_private_deps 1)
+  endif()
+
+  unset(PACKAGE_DEPENDENCIES)
+  if(DEFINED _IBPF_DEPENDENCIES)
+    set(PACKAGE_DEPENDENCIES "#### Expanded from @PACKAGE_DEPENDENCIES@ by install_basic_package_files() ####\n\ninclude(CMakeFindDependencyMacro)\n")
+
+    foreach(_dep ${_IBPF_DEPENDENCIES})
+      string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n")
     endforeach()
-    configure_package_config_file(${_config_cmake_in}
-                                  ${CMAKE_BINARY_DIR}/${_config_filename}.install
-                                  INSTALL_DESTINATION ${_IBPF_DESTINATION}
-                                  PATH_VARS ${_install_path_vars}
-                                  ${configure_package_config_file_extra_args})
-    install(FILES ${CMAKE_BINARY_DIR}/${_config_filename}.install
-            DESTINATION ${_IBPF_DESTINATION}
-            RENAME ${_config_filename})
 
+    if(_need_private_deps)
+      foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES})
+        string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n")
+      endforeach()
+    endif()
 
+    string(APPEND PACKAGE_DEPENDENCIES "\n###############################################################################\n")
+  endif()
 
-    # <name>Targets.cmake (build tree)
-    export(TARGETS ${_targets}
-           NAMESPACE ${_IBPF_NAMESPACE}
-           FILE ${CMAKE_BINARY_DIR}/${_targets_filename})
+  # Prepare PACKAGE_VERSION variable
+  set(PACKAGE_VERSION ${_IBPF_VERSION})
 
-    # <name>Targets.cmake (installed)
-    install(EXPORT ${_Name}
+  # <Name>Config.cmake (build tree)
+  foreach(p ${_build_path_vars_suffix})
+    if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p})
+      set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_BUILD_${p}}")
+    elseif(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p})
+      set(${_IBPF_VARS_PREFIX}_${p} "${BUILD_${_IBPF_VARS_PREFIX}_${p}}")
+    endif()
+  endforeach()
+  configure_package_config_file("${_config_cmake_in}"
+                                "${_IBPF_EXPORT_DESTINATION}/${_config_filename}"
+                                INSTALL_DESTINATION ${_IBPF_EXPORT_DESTINATION}
+                                PATH_VARS ${_build_path_vars}
+                                ${configure_package_config_file_extra_args}
+                                INSTALL_PREFIX ${CMAKE_BINARY_DIR})
+
+  # <Name>Config.cmake (installed)
+  foreach(p ${_install_path_vars_suffix})
+    if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p})
+      set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_INSTALL_${p}}")
+    elseif(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p})
+      set(${_IBPF_VARS_PREFIX}_${p} "${INSTALL_${_IBPF_VARS_PREFIX}_${p}}")
+    endif()
+  endforeach()
+  configure_package_config_file("${_config_cmake_in}"
+                                "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${_config_filename}.install"
+                                INSTALL_DESTINATION ${_IBPF_INSTALL_DESTINATION}
+                                PATH_VARS ${_install_path_vars}
+                                ${configure_package_config_file_extra_args})
+  install(FILES "${CMAKE_CURRENT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/${_config_filename}.install"
+          DESTINATION ${_IBPF_INSTALL_DESTINATION}
+          RENAME ${_config_filename}
+          COMPONENT ${_IBPF_COMPONENT})
+
+
+  # <Name>Targets.cmake (build tree)
+  if(NOT _IBPF_NO_EXPORT)
+    export(${_export_cmd}
+           NAMESPACE ${_IBPF_NAMESPACE}
+           FILE "${_IBPF_EXPORT_DESTINATION}/${_targets_filename}")
+  endif()
+
+  # Export build directory if CMAKE_EXPORT_PACKAGE_REGISTRY is set.
+  # CMake >= 3.15 already checks for CMAKE_EXPORT_PACKAGE_REGISTRY in `export(PACKAGE)` (cf.
+  # cf. https://cmake.org/cmake/help/latest/policy/CMP0090.html), and we effectively back-port
+  # this behavior to earlier versions.
+  if(CMAKE_EXPORT_PACKAGE_REGISTRY OR CMAKE_VERSION VERSION_GREATER_EQUAL 3.15)
+    export(PACKAGE ${_Name})
+  endif()
+
+  # <Name>Targets.cmake (installed)
+  if(NOT _IBPF_NO_EXPORT)
+    install(${_install_cmd}
             NAMESPACE ${_IBPF_NAMESPACE}
-            DESTINATION ${_IBPF_DESTINATION}
-            FILE ${_targets_filename})
-
+            DESTINATION ${_IBPF_INSTALL_DESTINATION}
+            FILE "${_targets_filename}"
+            COMPONENT ${_IBPF_COMPONENT})
+  endif()
 endfunction()
diff --git a/iDynTreeConfig.cmake.in b/iDynTreeConfig.cmake.in
deleted file mode 100644
index fef27568a31..00000000000
--- a/iDynTreeConfig.cmake.in
+++ /dev/null
@@ -1,36 +0,0 @@
-set(iDynTree_VERSION @iDynTree_VERSION@)
-
-@PACKAGE_INIT@
-
-if(NOT TARGET iDynTree::idyntree-core)
-  include("${CMAKE_CURRENT_LIST_DIR}/iDynTreeTargets.cmake")
-endif()
-
-if(TARGET iDynTree::idyntree-kdl)
-    find_package(orocos_kdl QUIET)
-    #support also for the old version of kdl cmake package
-    if(NOT orocos_kdl_FOUND)
-        find_package(Orocos-KDL QUIET)
-        if(NOT Orocos-KDL_FOUND)
-            message(WARNING "KDL not found: neither orocos_kdl or Orocos-KDL cmake packages are available")
-        else()
-            set(orocos_kdl_INCLUDE_DIRS ${Orocos-KDL_INCLUDE_DIRS})
-            set(orocos_kdl_LIBRARY_DIRS ${Orocos-KDL_LIBRARY_DIRS})
-            set(orocos_kdl_LIBRARIES ${Orocos-KDL_LIBRARIES})
-            set(orocos_kdl_FOUND true)
-        endif()
-    endif()
-    find_package(TinyXML QUIET)
-endif()
-
-if(TARGET iDynTree::idyntree-yarp)
-    find_package(YARP QUIET)
-endif()
-
-if(TARGET iDynTree::idyntree-icub)
-    find_package(ICUB QUIET)
-endif()
-
-# Provide an iDynTree_LIBRARIES variable containing all the
-# targets exported by iDynTree
-set(iDynTree_LIBRARIES "@iDynTree_TARGETS@")

From 82e018d3852b98e7dbd64db8f8dd4b7db1f421a3 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 21:56:43 +0100
Subject: [PATCH 171/194] Suppress warnings related to
 https://github.com/robotology/idyntree/issues/613

---
 cmake/InstallBasicPackageFiles.cmake | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/cmake/InstallBasicPackageFiles.cmake b/cmake/InstallBasicPackageFiles.cmake
index d00e80d9b03..aca51e5b6e9 100644
--- a/cmake/InstallBasicPackageFiles.cmake
+++ b/cmake/InstallBasicPackageFiles.cmake
@@ -321,7 +321,8 @@ function(INSTALL_BASIC_PACKAGE_FILES _Name)
     set(_export_cmd TARGETS ${_IBPF_TARGETS})
 
   elseif(DEFINED _IBPF_TARGETS_PROPERTY)
-    message(DEPRECATION "TARGETS_PROPERTY is deprecated. Use EXPORT instead")
+    # Warning suppressed for https://github.com/robotology/idyntree/issues/613
+    # message(DEPRECATION "TARGETS_PROPERTY is deprecated. Use EXPORT instead")
 
     if(DEFINED _IBPF_TARGETS_PROPERTIES)
       message(FATAL_ERROR "TARGETS_PROPERTIES cannot be used with TARGETS_PROPERTIES")
@@ -374,7 +375,8 @@ function(INSTALL_BASIC_PACKAGE_FILES _Name)
   endif()
   # ENABLE_COMPATIBILITY_VARS is deprecated
   if(_IBPF_ENABLE_COMPATIBILITY_VARS)
-    message(DEPRECATION "ENABLE_COMPATIBILITY_VARS is deprecated.")
+    # Warning for https://github.com/robotology/idyntree/issues/613 suppressed
+    # message(DEPRECATION "ENABLE_COMPATIBILITY_VARS is deprecated.")
   endif()
   # ENABLE_COMPATIBILITY_VARS does not work with EXPORT
   if(NOT DEFINED _targets AND _IBPF_ENABLE_COMPATIBILITY_VARS)

From 05029e02339b2b26fc873901884ee9bca40d221b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 22:00:03 +0100
Subject: [PATCH 172/194] Update CHANGELOG

---
 CHANGELOG.md | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 80a491c7acf..70da971d3f1 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -40,6 +40,7 @@ to normalize errors or as initial points of a nonlinear optimization procedure.
 
 ### Changed
 - The changelog has been migrated to the format described in https://keepachangelog.com/en/1.0.0/ .
+- The CMake config files are now installed in ${CMAKE_INSTALL_PREFIX}/lib/cmake/iDynTree also in Windows.
 
 ### Fixed
 - Fixed missing `DOF_ACCELLERATION` data in dynamic variable cache ordering

From 5571a3a1437c183ed30496d9872e3c4026177a15 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 22:07:46 +0100
Subject: [PATCH 173/194] Remove generic never called methods for SparseMatrix

Some methods of SparseMatrix need to be implemented specifically for the
required ordering, and are never instantiated for an order different from
either iDynTree::RowMajor or iDynTree::ColumnMajor.
By removing them, we reduce the amount of useless code, and we avoid spurious warnings.

Fix https://github.com/robotology/idyntree/issues/605
---
 src/core/src/SparseMatrix.cpp | 36 -----------------------------------
 1 file changed, 36 deletions(-)

diff --git a/src/core/src/SparseMatrix.cpp b/src/core/src/SparseMatrix.cpp
index f4fd3250831..dbb9b906aaf 100644
--- a/src/core/src/SparseMatrix.cpp
+++ b/src/core/src/SparseMatrix.cpp
@@ -312,42 +312,6 @@ namespace iDynTree {
         return it;
     }
 
-    // MARK: - Methods to be explicitly implemented (thus asserted false)
-
-    template <iDynTree::MatrixStorageOrdering ordering>
-    SparseMatrix<ordering>::SparseMatrix(unsigned rows, unsigned cols, const iDynTree::VectorDynSize& memoryReserveDescription)
-    : m_allocatedSize(0)
-    , m_rows(rows)
-    , m_columns(cols)
-    {
-        UNUSED(memoryReserveDescription);
-        assert(false);
-    }
-
-    template <iDynTree::MatrixStorageOrdering ordering>
-    double SparseMatrix<ordering>::operator()(unsigned int, unsigned int) const
-    {
-        assert(false);
-    }
-
-    template <iDynTree::MatrixStorageOrdering ordering>
-    double& SparseMatrix<ordering>::operator()(unsigned int, unsigned int)
-    {
-        assert(false);
-    }
-
-    template <iDynTree::MatrixStorageOrdering ordering>
-    void SparseMatrix<ordering>::resize(unsigned, unsigned, const iDynTree::VectorDynSize&)
-    {
-        assert(false);
-    }
-
-    template <iDynTree::MatrixStorageOrdering ordering>
-    void SparseMatrix<ordering>::setFromTriplets(iDynTree::Triplets&)
-    {
-        assert(false);
-    }
-
     // MARK: - Row-Major implementation
     template <>
     SparseMatrix<iDynTree::RowMajor>::SparseMatrix(unsigned rows, unsigned cols, const iDynTree::VectorDynSize& memoryReserveDescription)

From 71d43a8dd2d3045f0a5b6fc3cf6f01a52b426eb5 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 23:31:02 +0100
Subject: [PATCH 174/194] Remove all uses of IDYNTREE_TREE_INCLUDE_DIRS global
 variable

The IDYNTREE_TREE_INCLUDE_DIRS global variable dates back to CMake 2.8.7,
when the "modern CMake" propagation of include usage requiremements via
target_include_directories was not available, and one of the main iDynTree
authors had quite a limited understanding of CMake in general.

It used to contain all the include directories of iDynTree, but not it is not
necessary. The only place in which it was still useful was:
* Passing the include directories to SWIG. For this, now all the idyntree targets
  are inspected and the appropriate include directories are extracted from the
  target-specific INTERFACE_INCLUDE_DIRECTORIES properties.
* Passing to tests the location of the testModels.h header that contains the location
  of test model files. This usage has been substitute with a new HEADER library idyntree-testmodels,
  that is linked in all tests that need to use the testModels.h header.
---
 bindings/CMakeLists.txt                        | 18 +++++++++++++++---
 src/core/CMakeLists.txt                        |  4 ----
 src/estimation/CMakeLists.txt                  |  5 -----
 src/estimation/tests/CMakeLists.txt            |  5 ++---
 src/high-level/CMakeLists.txt                  |  6 ------
 src/high-level/tests/CMakeLists.txt            |  5 ++---
 src/icub/CMakeLists.txt                        |  9 ---------
 src/inverse-kinematics/CMakeLists.txt          |  1 -
 src/inverse-kinematics/tests/CMakeLists.txt    |  5 ++---
 src/legacy/estimation-kdl/CMakeLists.txt       |  7 -------
 src/legacy/high-level-kdl/CMakeLists.txt       |  8 --------
 src/legacy/high-level-kdl/tests/CMakeLists.txt |  9 ++++-----
 src/legacy/icub-kdl/CMakeLists.txt             |  8 --------
 src/legacy/icub-kdl/tests/CMakeLists.txt       |  5 ++---
 src/legacy/kdl/CMakeLists.txt                  |  8 --------
 src/legacy/yarp-kdl/CMakeLists.txt             |  6 ------
 src/model/CMakeLists.txt                       |  7 -------
 src/model_io/iKin-kdl/CMakeLists.txt           |  4 ----
 src/model_io/tests/CMakeLists.txt              |  4 +---
 src/model_io/urdf-kdl/CMakeLists.txt           |  6 ------
 .../urdfdom/urdf_parser/CMakeLists.txt         |  9 ---------
 src/model_io/urdf/CMakeLists.txt               |  6 ------
 src/model_io/urdf/tests/CMakeLists.txt         |  5 ++---
 src/model_io/xml/CMakeLists.txt                | 12 ++----------
 src/model_io/xml/tests/CMakeLists.txt          |  6 ++----
 src/optimalcontrol/CMakeLists.txt              |  2 --
 src/optimalcontrol/tests/CMakeLists.txt        |  3 +--
 src/regressors/CMakeLists.txt                  |  7 -------
 src/sensors/CMakeLists.txt                     | 10 ----------
 src/sensors/tests/CMakeLists.txt               |  2 +-
 src/solid-shapes/CMakeLists.txt                |  2 --
 src/tests/benchmark/CMakeLists.txt             |  6 ++----
 src/tests/data/CMakeLists.txt                  |  3 ++-
 src/tests/icub_consistency/CMakeLists.txt      |  8 +++-----
 src/tests/integration/CMakeLists.txt           | 14 ++++++--------
 src/tests/kdl_consistency/CMakeLists.txt       |  8 +++-----
 src/tests/kdl_tests/CMakeLists.txt             |  9 +++------
 src/tests/yarp_benchmark/CMakeLists.txt        |  6 ++----
 src/tests/yarp_kdl_consistency/CMakeLists.txt  |  4 +---
 src/visualization/CMakeLists.txt               |  6 ------
 src/visualization/tests/CMakeLists.txt         |  2 +-
 src/yarp/CMakeLists.txt                        |  6 ------
 42 files changed, 59 insertions(+), 207 deletions(-)

diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt
index 151131bdb40..16e974d7bf2 100644
--- a/bindings/CMakeLists.txt
+++ b/bindings/CMakeLists.txt
@@ -17,10 +17,22 @@ if(SWIG_FOUND OR IDYNTREE_USES_MATLAB OR IDYNTREE_USES_OCTAVE)
 
     set_source_files_properties(iDynTree.i PROPERTIES CPLUSPLUS ON)
 
-    get_property(IDYNTREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
     get_property(IDYNTREE_LIBRARIES GLOBAL PROPERTY ${VARS_PREFIX}_TARGETS)
 
-    include_directories(${IDYNTREE_INCLUDE_DIRS})
+    # From https://github.com/robotology/yarp/blob/v3.3.0/bindings/CMakeLists.txt#L49
+    foreach(_lib IN LISTS IDYNTREE_LIBRARIES)
+        get_property(_include_dirs TARGET ${_lib} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
+        foreach(_dir IN LISTS _include_dirs)
+            if("${_dir}" MATCHES "\$<BUILD_INTERFACE:(.+)>$")
+                include_directories("${CMAKE_MATCH_1}")
+            elseif("${_dir}" MATCHES "\$<INSTALL_INTERFACE:(.+)>$")
+                # Nothing to do
+            else()
+                include_directories(${_dir})
+            endif()
+        endforeach()
+    endforeach()
+
     link_libraries(${IDYNTREE_LIBRARIES})
 
     # Remove the deprecation warnings because by definition we always build bindings also of deprecated modules
@@ -55,4 +67,4 @@ if(IDYNTREE_USES_PYTHON OR
    if(NOT SWIG_FOUND)
        MESSAGE(FATAL_ERROR "Swig not found, impossible to compile or generate iDynTree bindings.")
    endif()
-endif()
\ No newline at end of file
+endif()
diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt
index 26ef3b8f629..3584e67a38d 100644
--- a/src/core/CMakeLists.txt
+++ b/src/core/CMakeLists.txt
@@ -141,10 +141,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
-
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
 endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/estimation/CMakeLists.txt b/src/estimation/CMakeLists.txt
index 68e1600e837..3ed9d0a0148 100644
--- a/src/estimation/CMakeLists.txt
+++ b/src/estimation/CMakeLists.txt
@@ -54,9 +54,6 @@ target_include_directories(${libraryname} PRIVATE SYSTEM ${EIGEN3_INCLUDE_DIR})
 
 target_link_libraries(${libraryname} idyntree-core idyntree-model idyntree-sensors idyntree-modelio-urdf)
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS})
 
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_ESTIMATION_HEADERS})
@@ -72,8 +69,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
 endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/estimation/tests/CMakeLists.txt b/src/estimation/tests/CMakeLists.txt
index ac883d6b56a..f8c8903c556 100644
--- a/src/estimation/tests/CMakeLists.txt
+++ b/src/estimation/tests/CMakeLists.txt
@@ -6,14 +6,13 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_estimation_test classname)
     set(testsrc ${classname}UnitTest.cpp)
     set(testbinary ${classname}UnitTest)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-model idyntree-estimation)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-model idyntree-estimation idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/high-level/CMakeLists.txt b/src/high-level/CMakeLists.txt
index 0f946c52035..889c7c94d54 100644
--- a/src/high-level/CMakeLists.txt
+++ b/src/high-level/CMakeLists.txt
@@ -25,9 +25,6 @@ include_directories(${libraryname} SYSTEM ${EIGEN3_INCLUDE_DIR} ${orocos_kdl_INC
 
 target_link_libraries(${libraryname} idyntree-core idyntree-model idyntree-sensors idyntree-modelio-urdf)
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_HIGH_LEVEL_HEADERS})
 
 install(TARGETS ${libraryname}
@@ -41,9 +38,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
 endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/high-level/tests/CMakeLists.txt b/src/high-level/tests/CMakeLists.txt
index d1582211068..7abd3c01fd3 100644
--- a/src/high-level/tests/CMakeLists.txt
+++ b/src/high-level/tests/CMakeLists.txt
@@ -1,11 +1,10 @@
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_unit_test_hl classname)
     set(testsrc ${classname}UnitTest.cpp)
     set(testbinary ${classname}UnitTest)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-high-level idyntree-modelio-urdf)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-high-level idyntree-modelio-urdf idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
     
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/icub/CMakeLists.txt b/src/icub/CMakeLists.txt
index 0bbb5052f22..4f1df75361c 100644
--- a/src/icub/CMakeLists.txt
+++ b/src/icub/CMakeLists.txt
@@ -26,13 +26,6 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
 set(libraryname idyntree-icub)
 add_library(${libraryname} ${iDynTree_ICUB_source} ${iDynTree_ICUB_header})
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
-# share headers with all iDynTree targets
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include")
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 set_target_properties(${libraryname} PROPERTIES PUBLIC_HEADER "${iDynTree_ICUB_header}")
 
 
@@ -58,8 +51,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS idyntree-icub)
 
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
 # Install headers in deprecated location
 install(DIRECTORY include/iDynTree/iCub
         DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/iDynTree")
diff --git a/src/inverse-kinematics/CMakeLists.txt b/src/inverse-kinematics/CMakeLists.txt
index 194ff7d7530..64b84af9009 100644
--- a/src/inverse-kinematics/CMakeLists.txt
+++ b/src/inverse-kinematics/CMakeLists.txt
@@ -48,7 +48,6 @@ install(TARGETS ${libraryname}
         PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
 
 
 if(IDYNTREE_COMPILE_TESTS)
diff --git a/src/inverse-kinematics/tests/CMakeLists.txt b/src/inverse-kinematics/tests/CMakeLists.txt
index b663ac9b11b..b2e74355389 100644
--- a/src/inverse-kinematics/tests/CMakeLists.txt
+++ b/src/inverse-kinematics/tests/CMakeLists.txt
@@ -1,11 +1,10 @@
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_ik_test classname)
   set(testsrc ${classname}UnitTest.cpp)
   set(testbinary ${classname}UnitTest)
   set(testname   UnitTest${classname})
   add_executable(${testbinary} ${testsrc})
-  target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-  target_link_libraries(${testbinary} idyntree-model idyntree-inverse-kinematics)
+  target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+  target_link_libraries(${testbinary} idyntree-model idyntree-inverse-kinematics idyntree-testmodels)
   add_test(NAME ${testname} COMMAND ${testbinary})
 
   if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/legacy/estimation-kdl/CMakeLists.txt b/src/legacy/estimation-kdl/CMakeLists.txt
index 9dddef74243..e4112bbac1c 100644
--- a/src/legacy/estimation-kdl/CMakeLists.txt
+++ b/src/legacy/estimation-kdl/CMakeLists.txt
@@ -33,15 +33,10 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR} ${orocos_kdl_INCLUDE_DIRS})
 
 target_link_libraries(${libraryname} idyntree-core idyntree-kdl idyntree-sensors idyntree-modelio-urdf-kdl idyntree-yarp-kdl ${orocos_kdl_LIBRARIES} ${TinyXML_LIBRARIES})
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 if((${CMAKE_CXX_COMPILER_ID} MATCHES "GNU") OR (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang"))
     target_compile_options(${libraryname} PRIVATE "-Wno-deprecated")
 endif()
 
-set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_ESTIMATION_KDL_HEADERS})
-
 install(TARGETS ${libraryname}
         EXPORT iDynTree
         COMPONENT runtime
@@ -52,5 +47,3 @@ install(TARGETS ${libraryname}
         PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Estimation/impl)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
diff --git a/src/legacy/high-level-kdl/CMakeLists.txt b/src/legacy/high-level-kdl/CMakeLists.txt
index 4336528a316..aedee378694 100644
--- a/src/legacy/high-level-kdl/CMakeLists.txt
+++ b/src/legacy/high-level-kdl/CMakeLists.txt
@@ -42,11 +42,6 @@ if (IDYNTREE_USES_KDL)
     target_link_libraries(${libraryname} idyntree-kdl idyntree-modelio-urdf-kdl idyntree-sensors idyntree-modelio-urdf-kdl ${TinyXML_LIBRARIES})
 endif()
 
-
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
-
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_HIGH_LEVEL_KDL_HEADERS})
 
 install(TARGETS ${libraryname}
@@ -60,9 +55,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
 if(IDYNTREE_COMPILE_TESTS AND IDYNTREE_USES_KDL)
     add_subdirectory(tests)
 endif()
diff --git a/src/legacy/high-level-kdl/tests/CMakeLists.txt b/src/legacy/high-level-kdl/tests/CMakeLists.txt
index d358a8025f9..bda2657cf3f 100644
--- a/src/legacy/high-level-kdl/tests/CMakeLists.txt
+++ b/src/legacy/high-level-kdl/tests/CMakeLists.txt
@@ -6,14 +6,13 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_unit_test_hl classname)
     set(testsrc ${classname}UnitTest.cpp)
     set(testbinary ${classname}UnitTest)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-high-level-kdl idyntree-yarp-kdl idyntree-yarp)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-high-level-kdl idyntree-yarp-kdl idyntree-yarp idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
     if(IDYNTREE_RUN_VALGRIND_TESTS)
        add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${testbinary}>)
@@ -25,8 +24,8 @@ macro(add_unit_test_hl_yarp classname)
     set(testbinary ${classname}UnitTest)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-high-level-kdl idyntree-yarp-kdl idyntree-yarp idyntree-high-level)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-high-level-kdl idyntree-yarp-kdl idyntree-yarp idyntree-high-level idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
     if(IDYNTREE_RUN_VALGRIND_TESTS)
        add_test(NAME memcheck_${testname} COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:${testbinary}>)
diff --git a/src/legacy/icub-kdl/CMakeLists.txt b/src/legacy/icub-kdl/CMakeLists.txt
index 6c788845dee..68f04b3f2ab 100644
--- a/src/legacy/icub-kdl/CMakeLists.txt
+++ b/src/legacy/icub-kdl/CMakeLists.txt
@@ -38,8 +38,6 @@ set_target_properties(${libraryname} PROPERTIES VERSION ${${VARS_PREFIX}_VERSION
                                            SOVERSION ${${VARS_PREFIX}_VERSION}
                                            PUBLIC_HEADER "${iDynTree_ICUB_KDL_header}")
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                 "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
@@ -67,10 +65,6 @@ include_directories(SYSTEM ${skinDynLib_INCLUDE_DIRS}
                            ${EIGEN3_INCLUDE_DIR}
                            ${orocos_kdl_INCLUDE_DIRS})
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
-
 if((${CMAKE_CXX_COMPILER_ID} MATCHES "GNU") OR (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang"))
     target_compile_options(${libraryname} PRIVATE "-Wno-deprecated")
 endif()
@@ -84,8 +78,6 @@ install(TARGETS idyntree-icub-kdl
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS idyntree-icub-kdl)
 
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
 
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
diff --git a/src/legacy/icub-kdl/tests/CMakeLists.txt b/src/legacy/icub-kdl/tests/CMakeLists.txt
index dda98636648..783abfe3e07 100644
--- a/src/legacy/icub-kdl/tests/CMakeLists.txt
+++ b/src/legacy/icub-kdl/tests/CMakeLists.txt
@@ -5,14 +5,13 @@ include_directories(${CMAKE_SOURCE_DIR}/include
                           ${YARP_INCLUDE_DIRS}
                           ${ICUB_INCLUDE_DIRS})
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_icub_test testName)
     set(testsrc ${testName}Test.cpp)
     set(testbinary ${testName}Test)
     set(testname   test_${testName})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-core idyntree-high-level idyntree-yarp-kdl idyntree-icub-kdl ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES})
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-core idyntree-high-level idyntree-yarp-kdl idyntree-icub-kdl ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES} idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/legacy/kdl/CMakeLists.txt b/src/legacy/kdl/CMakeLists.txt
index b4793a4d051..daa8a50668b 100644
--- a/src/legacy/kdl/CMakeLists.txt
+++ b/src/legacy/kdl/CMakeLists.txt
@@ -29,8 +29,6 @@ set(libraryname idyntree-kdl)
 
 add_library(${libraryname} ${IDYNTREE_KDL_SOURCES} ${IDYNTREE_KDL_HEADERS} ${IDYNTREE_KDL_PRIVATE_INCLUDES})
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
@@ -45,10 +43,6 @@ target_include_directories(${libraryname} INTERFACE ${EIGEN3_INCLUDE_DIR}
 include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}
                            ${orocos_kdl_INCLUDE_DIRS})
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
-
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_KDL_HEADERS})
 
 install(TARGETS ${libraryname}
@@ -60,5 +54,3 @@ install(TARGETS ${libraryname}
         PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/kdl_codyco/impl)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
diff --git a/src/legacy/yarp-kdl/CMakeLists.txt b/src/legacy/yarp-kdl/CMakeLists.txt
index a10248b5b2a..6bd038a8967 100644
--- a/src/legacy/yarp-kdl/CMakeLists.txt
+++ b/src/legacy/yarp-kdl/CMakeLists.txt
@@ -19,10 +19,6 @@ SET(iDynTree_YARP_KDL_header include/iCub/iDynTree/DynTree.h
 SOURCE_GROUP("Source Files" FILES ${iDynTree_YARP_KDL_source})
 SOURCE_GROUP("Header Files" FILES ${iDynTree_YARP_KDL_header})
 
-# share headers with all iDynTree targes
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include")
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 include(AddInstallRPATHSupport)
 add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
                           LIB_DIRS "${CMAKE_INSTALL_PREFIX}/lib"
@@ -42,8 +38,6 @@ set_target_properties(idyntree-yarp-kdl PROPERTIES VERSION ${${VARS_PREFIX}_VERS
 target_include_directories(idyntree-yarp-kdl PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                     "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
-
 target_include_directories(idyntree-yarp-kdl INTERFACE ${EIGEN3_INCLUDE_DIR}
                                                     ${YARP_INCLUDE_DIRS})
 include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}
diff --git a/src/model/CMakeLists.txt b/src/model/CMakeLists.txt
index c4701b49b0f..c88af94a594 100644
--- a/src/model/CMakeLists.txt
+++ b/src/model/CMakeLists.txt
@@ -72,10 +72,6 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
 
 target_link_libraries(${libraryname} idyntree-core)
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
-
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_MODEL_HEADERS})
 
 install(TARGETS ${libraryname}
@@ -89,9 +85,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
 
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
diff --git a/src/model_io/iKin-kdl/CMakeLists.txt b/src/model_io/iKin-kdl/CMakeLists.txt
index 38498691be5..e4faf51d56a 100644
--- a/src/model_io/iKin-kdl/CMakeLists.txt
+++ b/src/model_io/iKin-kdl/CMakeLists.txt
@@ -18,8 +18,6 @@ set(IDYNTREE_MODELIO_IKIN_KDL_SOURCES src/iKin_export.cpp)
 SOURCE_GROUP("Source Files" FILES ${IDYNTREE_MODELIO_IKIN_KDL_SOURCES})
 SOURCE_GROUP("Header Files" FILES ${IDYNTREE_MODELIO_IKIN_KDL_HEADERS})
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 set(libraryname idyntree-modelio-ikin-kdl)
 
 add_library(${libraryname} ${IDYNTREE_MODELIO_IKIN_KDL_SOURCES} ${IDYNTREE_MODELIO_IKIN_KDL_HEADERS})
@@ -44,5 +42,3 @@ install(TARGETS ${libraryname}
         PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/ModelIO)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
diff --git a/src/model_io/tests/CMakeLists.txt b/src/model_io/tests/CMakeLists.txt
index 4722185e237..b44767ec133 100644
--- a/src/model_io/tests/CMakeLists.txt
+++ b/src/model_io/tests/CMakeLists.txt
@@ -6,15 +6,13 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
 include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS})
 FIND_PACKAGE(Boost)
 include_directories(${Boost_INCLUDE_DIR})
 
 add_executable(check_urdf_import_export check_urdf_import_export.cpp)
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/format_examples/urdf/black_icub.urdf ${CMAKE_CURRENT_BINARY_DIR}/black_icub.urdf)
-target_link_libraries(check_urdf_import_export idyntree-kdl idyntree-modelio-urdf-kdl ${orocos_kdl_LIBRARIES} ${urdfdom_LIBRARIES})
+target_link_libraries(check_urdf_import_export idyntree-kdl idyntree-modelio-urdf-kdl ${orocos_kdl_LIBRARIES} ${urdfdom_LIBRARIES} idyntree-testmodels)
 add_test(NAME test_urdf_import_export COMMAND check_urdf_import_export black_icub.urdf)
 
 if (IDYNTREE_USES_SYMORO_PAR)
diff --git a/src/model_io/urdf-kdl/CMakeLists.txt b/src/model_io/urdf-kdl/CMakeLists.txt
index c31e7ee64c8..b36d084cb0c 100644
--- a/src/model_io/urdf-kdl/CMakeLists.txt
+++ b/src/model_io/urdf-kdl/CMakeLists.txt
@@ -49,9 +49,6 @@ add_library(${libraryname} ${IDYNTREE_MODELIO_URDF_SOURCES}
                            ${IDYNTREE_MODELIO_URDF_HEADERS}
                            ${IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS})
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
-
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
@@ -85,9 +82,6 @@ set_property(TARGET ${libraryname} PROPERTY PRIVATE_HEADER ${IDYNTREE_MODELIO_UR
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
 install(TARGETS ${libraryname}
     EXPORT iDynTree
     COMPONENT runtime
diff --git a/src/model_io/urdf-kdl/external/urdfdom/urdf_parser/CMakeLists.txt b/src/model_io/urdf-kdl/external/urdfdom/urdf_parser/CMakeLists.txt
index 55ed97a01a7..1fb98a5c4f5 100644
--- a/src/model_io/urdf-kdl/external/urdfdom/urdf_parser/CMakeLists.txt
+++ b/src/model_io/urdf-kdl/external/urdfdom/urdf_parser/CMakeLists.txt
@@ -1,14 +1,5 @@
 include_directories(include)
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-include_directories(SYSTEM ${IDYNTREE_TREE_INCLUDE_DIRS})
-
-# share headers with all iDynTree targes
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/urdf_parser/include")
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/../urdfdom_headers/urdf_model/include")
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/../urdfdom_headers/urdf_exception/include")
-
-
 #add_library(iDynTree_priv_urdfdom_world SHARED src/pose.cpp src/model.cpp src/link.cpp src/joint.cpp src/world.cpp)
 #target_link_libraries(iDynTree_priv_urdfdom_world ${TinyXML_LIBRARIES})
 #set_target_properties(iDynTree_priv_urdfdom_world PROPERTIES SOVERSION 0.3)
diff --git a/src/model_io/urdf/CMakeLists.txt b/src/model_io/urdf/CMakeLists.txt
index 8c45251f385..1e33128af50 100644
--- a/src/model_io/urdf/CMakeLists.txt
+++ b/src/model_io/urdf/CMakeLists.txt
@@ -86,9 +86,6 @@ target_include_directories(${libraryname} PRIVATE ${EIGEN3_INCLUDE_DIR})
 # Can be removed with CMake 3.12
 target_include_directories(${libraryname} PRIVATE $<TARGET_PROPERTY:idyntree-private-fpconv,INTERFACE_INCLUDE_DIRECTORIES>)
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
 
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_MODELIO_URDF_HEADERS})
 
@@ -102,9 +99,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
 endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/model_io/urdf/tests/CMakeLists.txt b/src/model_io/urdf/tests/CMakeLists.txt
index 8698f41d56a..74d1cb87cc3 100644
--- a/src/model_io/urdf/tests/CMakeLists.txt
+++ b/src/model_io/urdf/tests/CMakeLists.txt
@@ -6,14 +6,13 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_modelio_urdf_unit_test classname)
     set(testsrc ${classname}UnitTest.cpp)
     set(testbinary ${classname}UnitTest)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-modelio-urdf)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-modelio-urdf idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/model_io/xml/CMakeLists.txt b/src/model_io/xml/CMakeLists.txt
index b3903e0ba8d..5b9adc487bf 100644
--- a/src/model_io/xml/CMakeLists.txt
+++ b/src/model_io/xml/CMakeLists.txt
@@ -36,10 +36,9 @@ add_library(${libraryname} ${IDYNTREE_MODELIO_XML_SOURCES} ${IDYNTREE_MODELIO_XM
 # Test if this works:
 # We want to include the same-library header files directly (in the implementation), but with the full prefix in the .h (as this will be public)
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/iDynTree>"
-												 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/private>"
+                                                 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/iDynTree>"
+                                                 "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/private>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>"
-                                                  ${IDYNTREE_TREE_INCLUDE_DIRS}
                                           PRIVATE ${EIGEN3_INCLUDE_DIR}
                                                   ${LIBXML2_INCLUDE_DIR})
 
@@ -50,10 +49,6 @@ if(NOT CMAKE_VERSION VERSION_LESS 2.8.12)
     target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS} ${LIBXML2_DEFINITIONS})
 endif()
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
-
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_MODELIO_XML_HEADERS})
 
 install(TARGETS ${libraryname}
@@ -66,9 +61,6 @@ install(TARGETS ${libraryname}
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
 
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
-
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
 endif(IDYNTREE_COMPILE_TESTS)
diff --git a/src/model_io/xml/tests/CMakeLists.txt b/src/model_io/xml/tests/CMakeLists.txt
index a3106f8ec3b..3bbcd79f07d 100644
--- a/src/model_io/xml/tests/CMakeLists.txt
+++ b/src/model_io/xml/tests/CMakeLists.txt
@@ -6,17 +6,15 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 set(IDYNTREE_MODELIO_XML_DATAFILES ${IDYNTREE_MODELIO_TESTFILES_DIR}/xml)
 
 add_executable(XMLParserUnitTest XMLParserUnitTest.cpp)
 target_compile_definitions(XMLParserUnitTest PUBLIC -DIDYNTREE_TEST_FILES_DIR="${IDYNTREE_MODELIO_XML_DATAFILES}")
 
-target_include_directories(XMLParserUnitTest PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
+target_include_directories(XMLParserUnitTest PRIVATE ${EIGEN3_INCLUDE_DIR})
 target_link_libraries(XMLParserUnitTest idyntree-core idyntree-modelio-xml)
 add_test(NAME UnitTestXMLParser COMMAND XMLParserUnitTest)
 
 if(IDYNTREE_RUN_VALGRIND_TESTS)
     add_test(NAME memcheck_UnitTestXMLParser COMMAND ${MEMCHECK_COMMAND_COMPLETE} $<TARGET_FILE:XMLParserUnitTest>)
-endif()
\ No newline at end of file
+endif()
diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index c2bc2732921..5a435790be6 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -137,8 +137,6 @@ install(FILES ${OCSOLVERS_PUBLIC_HEADERS} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR
 
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
 
 if(IDYNTREE_COMPILE_TESTS)
   add_subdirectory(tests)
diff --git a/src/optimalcontrol/tests/CMakeLists.txt b/src/optimalcontrol/tests/CMakeLists.txt
index fea8dee80fd..cc8c2045441 100644
--- a/src/optimalcontrol/tests/CMakeLists.txt
+++ b/src/optimalcontrol/tests/CMakeLists.txt
@@ -6,13 +6,12 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
 macro(add_oc_test classname)
     set(testsrc ${classname}Test.cpp)
     set(testbinary ${classname}UnitTest)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
     target_link_libraries(${testbinary} idyntree-model idyntree-optimalcontrol)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
diff --git a/src/regressors/CMakeLists.txt b/src/regressors/CMakeLists.txt
index 5a3487d4afc..e461a399be6 100644
--- a/src/regressors/CMakeLists.txt
+++ b/src/regressors/CMakeLists.txt
@@ -46,10 +46,6 @@ if (IDYNTREE_USES_KDL)
   target_link_libraries(${libraryname} idyntree-modelio-urdf-kdl ${orocos_kdl_LIBRARIES} ${TinyXML_LIBRARIES})
 endif ()
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
-
 if((${CMAKE_CXX_COMPILER_ID} MATCHES "GNU") OR (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang"))
     target_compile_options(${libraryname} PRIVATE "-Wno-deprecated")
 endif()
@@ -68,6 +64,3 @@ install(TARGETS ${libraryname}
 install(DIRECTORY include/iDynTree/Regressors DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-
-get_property(IDYNTREE_COMP_INCLUDE_DIRS TARGET ${libraryname} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${IDYNTREE_COMP_INCLUDE_DIRS})
diff --git a/src/sensors/CMakeLists.txt b/src/sensors/CMakeLists.txt
index 3115e8bd468..05b82cfc75c 100644
--- a/src/sensors/CMakeLists.txt
+++ b/src/sensors/CMakeLists.txt
@@ -8,12 +8,6 @@
 
 project(iDynTree_Sensors CXX)
 
-# share headers with all iDynTree targets
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include")
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
-
 set(IDYNTREE_SENSORS_HEADERS include/iDynTree/Sensors/AllSensorsTypes.h
                              include/iDynTree/Sensors/Sensors.h
                              include/iDynTree/Sensors/SixAxisForceTorqueSensor.h
@@ -52,9 +46,6 @@ include_directories(SYSTEM AFTER ${orocos_kdl_INCLUDE_DIRS}
 
 target_link_libraries(${libraryname} idyntree-core idyntree-model)
 
-# Ensure that build include directories are always included before system ones
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-target_include_directories(${libraryname} BEFORE PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS})
 
 target_compile_options(${libraryname} PRIVATE ${IDYNTREE_WARNING_FLAGS})
 
@@ -70,7 +61,6 @@ install(TARGETS ${libraryname}
         PRIVATE_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree/Sensors/Impl)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
 
 if(IDYNTREE_COMPILE_TESTS)
     add_subdirectory(tests)
diff --git a/src/sensors/tests/CMakeLists.txt b/src/sensors/tests/CMakeLists.txt
index 4201db03e68..d6c12a9d261 100644
--- a/src/sensors/tests/CMakeLists.txt
+++ b/src/sensors/tests/CMakeLists.txt
@@ -4,7 +4,7 @@ macro(add_unit_test classname)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-sensors)
+    target_link_libraries(${testbinary} idyntree-sensors idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/solid-shapes/CMakeLists.txt b/src/solid-shapes/CMakeLists.txt
index 73d80c0b51f..3c10dd0e8b3 100644
--- a/src/solid-shapes/CMakeLists.txt
+++ b/src/solid-shapes/CMakeLists.txt
@@ -31,8 +31,6 @@ install(TARGETS ${libraryname}
         PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/iDynTree)
 
 set_property(GLOBAL APPEND PROPERTY ${VARS_PREFIX}_TARGETS ${libraryname})
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
-
 
 if(IDYNTREE_COMPILE_TESTS AND IDYNTREE_USES_ASSIMP)
     add_subdirectory(tests)
diff --git a/src/tests/benchmark/CMakeLists.txt b/src/tests/benchmark/CMakeLists.txt
index 0df14d67b3f..348d875f657 100644
--- a/src/tests/benchmark/CMakeLists.txt
+++ b/src/tests/benchmark/CMakeLists.txt
@@ -6,16 +6,14 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 macro(add_benchmark benchmarkName)
     set(testsrc ${benchmarkName}Benchmark.cpp)
     set(testbinary ${benchmarkName}Benchmark)
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
     FIND_PACKAGE(Boost)
     include_directories(${Boost_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-modelio-urdf-kdl idyntree-core idyntree-kdl idyntree-model)
+    target_link_libraries(${testbinary} idyntree-modelio-urdf-kdl idyntree-core idyntree-kdl idyntree-model idyntree-testmodels)
 endmacro()
 
 add_benchmark(Dynamics)
diff --git a/src/tests/data/CMakeLists.txt b/src/tests/data/CMakeLists.txt
index d6510c1d0ec..bd6240a6ce9 100644
--- a/src/tests/data/CMakeLists.txt
+++ b/src/tests/data/CMakeLists.txt
@@ -35,4 +35,5 @@ set(IDYNTREE_TESTS_URDFS icub.urdf
 # configure an header containing the path
 # where the test files are stored and the list of files
 configure_file("${CMAKE_CURRENT_SOURCE_DIR}/testModels.h.in" "${CMAKE_CURRENT_BINARY_DIR}/testModels.h" @ONLY)
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${CMAKE_CURRENT_BINARY_DIR}")
\ No newline at end of file
+add_library(idyntree-testmodels INTERFACE)
+target_include_directories(idyntree-testmodels INTERFACE "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>")
diff --git a/src/tests/icub_consistency/CMakeLists.txt b/src/tests/icub_consistency/CMakeLists.txt
index 24abaaa0acb..f2cd11aa97a 100644
--- a/src/tests/icub_consistency/CMakeLists.txt
+++ b/src/tests/icub_consistency/CMakeLists.txt
@@ -6,15 +6,13 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
 
 macro(add_icub_consistency_test_kdl testName)
     set(testsrc ${testName}ConsistencyTest.cpp)
     set(testbinary ${testName}ConsistencyTest)
     set(testname   ConsistencyTest${testName})
     add_executable(${testbinary} ${testsrc})
-    target_link_libraries(${testbinary} idyntree-modelio-urdf-kdl idyntree-yarp idyntree-high-level idyntree-modelio-ikin-kdl idyntree-icub idyntree-icub-kdl idyntree-icub idyntree-core idyntree-kdl idyntree-model idyntree-estimation)
+    target_link_libraries(${testbinary} idyntree-modelio-urdf-kdl idyntree-yarp idyntree-high-level idyntree-modelio-ikin-kdl idyntree-icub idyntree-icub-kdl idyntree-icub idyntree-core idyntree-kdl idyntree-model idyntree-estimation idyntree-testmodels)
     target_include_directories(${testbinary} PRIVATE SYSTEM ${EIGEN3_INCLUDE_DIR})
     add_test(NAME ${testname} COMMAND ${testbinary})
 
@@ -29,7 +27,7 @@ macro(add_icub_consistency_test testName)
     set(testbinary ${testName}ConsistencyTest)
     set(testname   ConsistencyTest${testName})
     add_executable(${testbinary} ${testsrc})
-    target_link_libraries(${testbinary} idyntree-yarp idyntree-high-level idyntree-icub idyntree-icub idyntree-icub idyntree-core idyntree-model idyntree-estimation)
+    target_link_libraries(${testbinary} idyntree-yarp idyntree-high-level idyntree-icub idyntree-icub idyntree-icub idyntree-core idyntree-model idyntree-estimation idyntree-testmodels)
     target_include_directories(${testbinary} PRIVATE SYSTEM ${EIGEN3_INCLUDE_DIR})
     add_test(NAME ${testname} COMMAND ${testbinary})
 
@@ -42,4 +40,4 @@ endmacro()
 add_icub_consistency_test(iKin)
 if (IDYNTREE_USES_KDL)
     add_icub_consistency_test_kdl(iCubExternalWrenchesEstimation)
-endif ()
\ No newline at end of file
+endif ()
diff --git a/src/tests/integration/CMakeLists.txt b/src/tests/integration/CMakeLists.txt
index cf6c4ac5e6c..d86de3ebb96 100644
--- a/src/tests/integration/CMakeLists.txt
+++ b/src/tests/integration/CMakeLists.txt
@@ -6,15 +6,13 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 macro(add_integration_test testname)
     set(testsrc ${testname}IntegrationTest.cpp)
     set(testbinary ${testname}IntegrationTest)
     set(testtarget   IntegrationTest${testname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation idyntree-solid-shapes)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation idyntree-solid-shapes idyntree-testmodels)
     # Setting explicitly the WORKING_DIRECTORY is necessary to make sure that meshes are correctly loaded,
     # as a workaround for https://github.com/robotology/idyntree/issues/291
     add_test(NAME ${testtarget} COMMAND ${testbinary} WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/src/tests/data)
@@ -29,8 +27,8 @@ macro(add_integration_test_no_valgrind testname)
     set(testbinary ${testname}IntegrationTest)
     set(testtarget   IntegrationTest${testname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation  idyntree-solid-shapes)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-sensors idyntree-estimation  idyntree-solid-shapes idyntree-testmodels)
     add_test(NAME ${testtarget} COMMAND ${testbinary})
 endmacro()
 
@@ -39,8 +37,8 @@ macro(add_integration_exe testname)
     set(testbinary ${testname}IntegrationTest)
     set(testtarget   IntegrationTest${testname})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-core idyntree-model idyntree-modelio-urdf idyntree-high-level idyntree-testmodels)
 endmacro()
 
 add_integration_test(Dynamics)
diff --git a/src/tests/kdl_consistency/CMakeLists.txt b/src/tests/kdl_consistency/CMakeLists.txt
index 6d868a8f9c9..ee38f6b6607 100644
--- a/src/tests/kdl_consistency/CMakeLists.txt
+++ b/src/tests/kdl_consistency/CMakeLists.txt
@@ -6,8 +6,6 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 # Legacy tests, remove deprecated warnings when compiling
 idyntree_disable_deprecation_warnings()
 
@@ -16,8 +14,8 @@ macro(add_kdl_consistency_test testName)
     set(testbinary ${testName}ConsistencyTest)
     set(testname   ConsistencyTest${testName})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-modelio-urdf-kdl idyntree-core idyntree-kdl idyntree-model)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-modelio-urdf-kdl idyntree-core idyntree-kdl idyntree-model idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
@@ -28,4 +26,4 @@ endmacro()
 
 
 add_kdl_consistency_test(Geometry)
-add_kdl_consistency_test(KinematicsDynamics)
\ No newline at end of file
+add_kdl_consistency_test(KinematicsDynamics)
diff --git a/src/tests/kdl_tests/CMakeLists.txt b/src/tests/kdl_tests/CMakeLists.txt
index d378b3961c5..1887c223420 100644
--- a/src/tests/kdl_tests/CMakeLists.txt
+++ b/src/tests/kdl_tests/CMakeLists.txt
@@ -6,12 +6,9 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 # Legacy tests, remove deprecated warnings when compiling
 idyntree_disable_deprecation_warnings()
 
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
 include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR} ${orocos_kdl_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS})
 
 macro(add_all_components_test testName)
@@ -19,8 +16,8 @@ macro(add_all_components_test testName)
     set(testbinary ${testName}_test)
     set(testname   test_${testName})
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
-    target_link_libraries(${testbinary} idyntree-core idyntree-high-level-kdl idyntree-yarp-kdl ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES})
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-core idyntree-high-level-kdl idyntree-yarp-kdl ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES} idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
@@ -48,7 +45,7 @@ if(IDYNTREE_USES_ICUB_MAIN)
   find_package(ICUB)
   add_executable(urdf2dh_standalone_test urdf2dh_test.cpp)
   target_include_directories(urdf2dh_standalone_test PUBLIC ${ICUB_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS})
-  target_link_libraries(urdf2dh_standalone_test iKin idyntree-kdl idyntree-yarp-kdl idyntree-regressors idyntree-modelio-urdf-kdl idyntree-modelio-ikin-kdl)
+  target_link_libraries(urdf2dh_standalone_test iKin idyntree-kdl idyntree-yarp-kdl idyntree-regressors idyntree-modelio-urdf-kdl idyntree-modelio-ikin-kdl idyntree-testmodels)
 # \todo reenable this test
   #add_test(NAME test_urdf2dh COMMAND urdf2dh_test urdf_icub_test.urdf)
 endif()
diff --git a/src/tests/yarp_benchmark/CMakeLists.txt b/src/tests/yarp_benchmark/CMakeLists.txt
index 2e572f83b83..53d70d50aa1 100644
--- a/src/tests/yarp_benchmark/CMakeLists.txt
+++ b/src/tests/yarp_benchmark/CMakeLists.txt
@@ -6,14 +6,12 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 macro(add_yarp_benchmark benchmarkName)
     set(testsrc ${benchmarkName}Benchmark.cpp)
     set(testbinary ${benchmarkName}Benchmark)
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${IDYNTREE_TREE_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-icub idyntree-core idyntree-model)
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
+    target_link_libraries(${testbinary} idyntree-icub idyntree-core idyntree-model idyntree-testmodels)
 endmacro()
 
 add_yarp_benchmark(PseudoInverse)
diff --git a/src/tests/yarp_kdl_consistency/CMakeLists.txt b/src/tests/yarp_kdl_consistency/CMakeLists.txt
index 9bfdac09c30..a116e0dc468 100644
--- a/src/tests/yarp_kdl_consistency/CMakeLists.txt
+++ b/src/tests/yarp_kdl_consistency/CMakeLists.txt
@@ -6,14 +6,12 @@
 # https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
 # at your option.
 
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 macro(add_yarp_kdl_consistency_test testName)
     set(testsrc ${testName}Consistency.cpp)
     set(testbinary ${testName}ConsistencyTest)
     set(testname   ${testName}ConsistencyTest)
     add_executable(${testbinary} ${testsrc})
-    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR} ${IDYNTREE_TREE_INCLUDE_DIRS})
+    target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
     target_link_libraries(${testbinary}  idyntree-modelio-urdf-kdl idyntree-yarp-kdl idyntree-yarp idyntree-core idyntree-kdl idyntree-model ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES})
     add_test(NAME ${testname} COMMAND ${testbinary})
 
diff --git a/src/visualization/CMakeLists.txt b/src/visualization/CMakeLists.txt
index 09aae9b65bc..fbc94e756bc 100644
--- a/src/visualization/CMakeLists.txt
+++ b/src/visualization/CMakeLists.txt
@@ -32,10 +32,6 @@ source_group("Header Files" FILES ${iDynTree_visualization_header})
 source_group("Private Source Files" FILES ${iDynTree_visualization_source})
 source_group("Private Header Files" FILES ${iDynTree_visualization_header})
 
-# share headers with all iDynTree targets
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include")
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 include(AddInstallRPATHSupport)
 add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
                           LIB_DIRS "${CMAKE_INSTALL_PREFIX}/lib"
@@ -53,8 +49,6 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>"
                                           PRIVATE ${EIGEN3_INCLUDE_DIR})
 
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
-
 target_link_libraries(${libraryname} LINK_PUBLIC idyntree-core
                                                  idyntree-model)
 
diff --git a/src/visualization/tests/CMakeLists.txt b/src/visualization/tests/CMakeLists.txt
index 74c583e10a8..6185fd1444c 100644
--- a/src/visualization/tests/CMakeLists.txt
+++ b/src/visualization/tests/CMakeLists.txt
@@ -12,7 +12,7 @@ macro(add_unit_test classname)
     set(testname   UnitTest${classname})
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-sensors idyntree-modelio-urdf idyntree-visualization)
+    target_link_libraries(${testbinary} idyntree-sensors idyntree-modelio-urdf idyntree-visualization idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/yarp/CMakeLists.txt b/src/yarp/CMakeLists.txt
index 1429ae8539e..290ba19982a 100644
--- a/src/yarp/CMakeLists.txt
+++ b/src/yarp/CMakeLists.txt
@@ -18,10 +18,6 @@ SET(iDynTree_YARP_header include/iDynTree/yarp/YARPConversions.h
 SOURCE_GROUP("Source Files" FILES ${iDynTree_YARP_source})
 SOURCE_GROUP("Header Files" FILES ${iDynTree_YARP_header})
 
-# share headers with all iDynTree targets
-set_property(GLOBAL APPEND PROPERTY IDYNTREE_TREE_INCLUDE_DIRS "${CMAKE_CURRENT_SOURCE_DIR}/include")
-get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
-
 include(AddInstallRPATHSupport)
 add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
                           LIB_DIRS "${CMAKE_INSTALL_PREFIX}/lib"
@@ -36,8 +32,6 @@ set_target_properties(idyntree-yarp PROPERTIES PUBLIC_HEADER "${iDynTree_YARP_he
 target_include_directories(idyntree-yarp PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                 "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
-include_directories(${IDYNTREE_TREE_INCLUDE_DIRS})
-
 target_include_directories(idyntree-yarp INTERFACE ${YARP_INCLUDE_DIRS})
 
 target_include_directories(idyntree-yarp PRIVATE SYSTEM ${YARP_INCLUDE_DIRS})

From ae7f7b85df5abc6d2e668d50ed15fdd3bf3046d4 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 23:42:47 +0100
Subject: [PATCH 175/194] Fixup

---
 CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 09acb723216..be73d6bf755 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -73,7 +73,6 @@ if(NOT IDYNTREE_ONLY_DOCS)
     endif()
     if(IDYNTREE_USES_KDL)
         list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES orocos_kdl)
-        list(APPEND _IDYNTREE_EXPORTED_DEPENDENCIES TinyXML)
     endif()
 
     include(InstallBasicPackageFiles)

From c6b9ec33ed2819d24e867cdc5183eb05865d4526 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 09:33:37 +0100
Subject: [PATCH 176/194] Fix handling of ALGLIB dependency

The minimum version needs to be specified in idyntree_handle_dependency via the MINIMUM_VERSION option
---
 cmake/iDynTreeDependencies.cmake | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 91093a6e487..f0b801cb7f6 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -64,7 +64,7 @@ idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
-idyntree_handle_dependency(ALGLIB 3.14.0)
+idyntree_handle_dependency(ALGLIB MINIMUM_VERSION 3.14.0)
 idyntree_handle_dependency(WORHP)
 # Workaround for https://github.com/robotology/idyntree/issues/599
 idyntree_handle_dependency(ASSIMP DO_NOT_SILENTLY_SEARCH)

From 07283ea6839185fa2395fc6a6b037c5ffa17bbfb Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 09:48:51 +0100
Subject: [PATCH 177/194] Update iDynTreeDependencies.cmake

---
 cmake/iDynTreeDependencies.cmake | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index f0b801cb7f6..f8fdf979a9a 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -64,7 +64,13 @@ idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
 idyntree_handle_dependency(OsqpEigen)
-idyntree_handle_dependency(ALGLIB MINIMUM_VERSION 3.14.0)
+idyntree_handle_dependency(ALGLIB DO_NOT_SILENTLY_SEARCH)
+set(ALGLIB_REQUIRED_VERSION 3.14.0)
+if(IDYNTREE_USES_ALGLIB AND ALGLIB_FOUND)
+  if(NOT ${ALGLIB_VERSION} VERSION_EQUAL ${ALGLIB_REQUIRED_VERSION})
+    message(FATAL_ERROR "Exactly ALGLIB version ${ALGLIB_REQUIRED_VERSION} is required, but ${ALGLIB_VERSION} is available.")
+  endif()
+endif()
 idyntree_handle_dependency(WORHP)
 # Workaround for https://github.com/robotology/idyntree/issues/599
 idyntree_handle_dependency(ASSIMP DO_NOT_SILENTLY_SEARCH)

From d176c23d8c6c98081a626c9bbdfceb5894b3064b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 10:01:25 +0100
Subject: [PATCH 178/194] Add definition of namespaced targets even for the
 build tree

It is useful to define an alias of the iDynTree libraries with the scope of the exported
library itself (i.e. iDynTree::<lib>) because (1) you can link against it with the exact same syntax
of an imported library, that is useful when iDynTree is used in a bigger project
via add_subdirectory and (2) because names having a double-colon (::) are
always treated as the name of either an alias or imported target. Any attempt
to use such a name for a different target type will result in an error.
---
 src/core/CMakeLists.txt               | 1 +
 src/estimation/CMakeLists.txt         | 1 +
 src/high-level/CMakeLists.txt         | 1 +
 src/icub/CMakeLists.txt               | 1 +
 src/inverse-kinematics/CMakeLists.txt | 1 +
 src/model_io/urdf/CMakeLists.txt      | 1 +
 src/model_io/xml/CMakeLists.txt       | 1 +
 src/optimalcontrol/CMakeLists.txt     | 2 ++
 src/regressors/CMakeLists.txt         | 1 +
 src/sensors/CMakeLists.txt            | 1 +
 src/solid-shapes/CMakeLists.txt       | 1 +
 src/visualization/CMakeLists.txt      | 1 +
 src/yarp/CMakeLists.txt               | 1 +
 13 files changed, 14 insertions(+)

diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt
index 26ef3b8f629..038620a833c 100644
--- a/src/core/CMakeLists.txt
+++ b/src/core/CMakeLists.txt
@@ -109,6 +109,7 @@ include_directories(include/iDynTree/Core)
 set(libraryname idyntree-core)
 
 add_library(${libraryname} ${IDYNTREE_CORE_EXP_SOURCES} ${IDYNTREE_CORE_EXP_HEADERS} ${IDYNTREE_CORE_EXP_PRIVATE_INCLUDES})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 if (DEFINED CMAKE_COMPILER_IS_GNUCXX)
   if(${CMAKE_COMPILER_IS_GNUCXX} AND ${CMAKE_CXX_COMPILER_VERSION} VERSION_LESS 5)
diff --git a/src/estimation/CMakeLists.txt b/src/estimation/CMakeLists.txt
index 68e1600e837..8aaed62326a 100644
--- a/src/estimation/CMakeLists.txt
+++ b/src/estimation/CMakeLists.txt
@@ -47,6 +47,7 @@ SOURCE_GROUP("Header Files" FILES ${IDYNTREE_ESTIMATION_HEADERS})
 set(libraryname idyntree-estimation)
 
 add_library(${libraryname} ${IDYNTREE_ESTIMATION_SOURCES} ${IDYNTREE_ESTIMATION_HEADERS} ${IDYNTREE_ESTIMATION_PRIVATE_INCLUDES})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
diff --git a/src/high-level/CMakeLists.txt b/src/high-level/CMakeLists.txt
index 0f946c52035..b7e4c603252 100644
--- a/src/high-level/CMakeLists.txt
+++ b/src/high-level/CMakeLists.txt
@@ -18,6 +18,7 @@ SOURCE_GROUP("Header Files" FILES ${IDYNTREE_HIGH_LEVEL_HEADERS})
 set(libraryname idyntree-high-level)
 
 add_library(${libraryname} ${IDYNTREE_HIGH_LEVEL_SOURCES} ${IDYNTREE_HIGH_LEVEL_HEADERS} ${IDYNTREE_HIGH_LEVEL_PRIVATE_INCLUDES})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
diff --git a/src/icub/CMakeLists.txt b/src/icub/CMakeLists.txt
index 0bbb5052f22..6c9b52303d3 100644
--- a/src/icub/CMakeLists.txt
+++ b/src/icub/CMakeLists.txt
@@ -25,6 +25,7 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
 
 set(libraryname idyntree-icub)
 add_library(${libraryname} ${iDynTree_ICUB_source} ${iDynTree_ICUB_header})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 # Ensure that build include directories are always included before system ones
 get_property(IDYNTREE_TREE_INCLUDE_DIRS GLOBAL PROPERTY IDYNTREE_TREE_INCLUDE_DIRS)
diff --git a/src/inverse-kinematics/CMakeLists.txt b/src/inverse-kinematics/CMakeLists.txt
index 194ff7d7530..b893267bf7d 100644
--- a/src/inverse-kinematics/CMakeLists.txt
+++ b/src/inverse-kinematics/CMakeLists.txt
@@ -23,6 +23,7 @@ source_group("Private\\Source Files" FILES ${PRIVATE_IDYN_TREE_IK_SOURCES})
 
 add_library(${libraryname} ${IDYN_TREE_IK_HEADERS} ${IDYN_TREE_IK_SOURCES}
                            ${PRIVATE_IDYN_TREE_IK_SOURCES} ${PRIVATE_IDYN_TREE_IK_HEADERS})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
diff --git a/src/model_io/urdf/CMakeLists.txt b/src/model_io/urdf/CMakeLists.txt
index 8c45251f385..09350fa3cc3 100644
--- a/src/model_io/urdf/CMakeLists.txt
+++ b/src/model_io/urdf/CMakeLists.txt
@@ -66,6 +66,7 @@ list(APPEND IDYNTREE_MODELIO_URDF_SOURCES ${IDYNTREE_MODELIO_URDF_XMLELEMENTS_SO
 set(libraryname idyntree-modelio-urdf)
 
 add_library(${libraryname} ${IDYNTREE_MODELIO_URDF_SOURCES} ${IDYNTREE_MODELIO_URDF_HEADERS} ${IDYNTREE_MODELIO_URDF_PRIVATE_HEADERS} $<TARGET_OBJECTS:idyntree-private-fpconv>)
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_compile_features(${libraryname} PRIVATE cxx_auto_type cxx_delegating_constructors cxx_final cxx_lambdas cxx_lambda_init_captures)
 
diff --git a/src/model_io/xml/CMakeLists.txt b/src/model_io/xml/CMakeLists.txt
index b3903e0ba8d..5ea96bb9b59 100644
--- a/src/model_io/xml/CMakeLists.txt
+++ b/src/model_io/xml/CMakeLists.txt
@@ -32,6 +32,7 @@ SOURCE_GROUP("Private Header Files" FILES ${IDYNTREE_MODELIO_XML_PRIVATE_HEADERS
 set(libraryname idyntree-modelio-xml)
 
 add_library(${libraryname} ${IDYNTREE_MODELIO_XML_SOURCES} ${IDYNTREE_MODELIO_XML_HEADERS} ${IDYNTREE_MODELIO_XML_PRIVATE_HEADERS})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 # Test if this works:
 # We want to include the same-library header files directly (in the implementation), but with the full prefix in the .h (as this will be public)
diff --git a/src/optimalcontrol/CMakeLists.txt b/src/optimalcontrol/CMakeLists.txt
index c2bc2732921..90d7e7abe25 100644
--- a/src/optimalcontrol/CMakeLists.txt
+++ b/src/optimalcontrol/CMakeLists.txt
@@ -109,6 +109,8 @@ else()
 endif()
 
 add_library(${libraryname} ${PUBLIC_HEADERS} ${INTEGRATORS_PUBLIC_HEADERS} ${OCSOLVERS_PUBLIC_HEADERS} ${OPTIMIZERS_HEADERS} ${SOURCES} ${OPTIMIZERS_SOURCES})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
+
 target_include_directories(${libraryname} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
 target_include_directories(${libraryname} SYSTEM PRIVATE ${INCLUDE_LIST})
 target_include_directories(${libraryname} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
diff --git a/src/regressors/CMakeLists.txt b/src/regressors/CMakeLists.txt
index 5a3487d4afc..832a59d1cee 100644
--- a/src/regressors/CMakeLists.txt
+++ b/src/regressors/CMakeLists.txt
@@ -36,6 +36,7 @@ set(libraryname idyntree-regressors)
 
 add_library(${libraryname} ${IDYNTREE_REGRESSORS_SOURCES}     ${IDYNTREE_REGRESSORS_HEADERS}
                                 ${IDYNTREE_REGRESSORS_SOURCES_EXP} ${IDYNTREE_REGRESSORS_HEADERS_EXP})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
diff --git a/src/sensors/CMakeLists.txt b/src/sensors/CMakeLists.txt
index 3115e8bd468..6f6a1c0cc6c 100644
--- a/src/sensors/CMakeLists.txt
+++ b/src/sensors/CMakeLists.txt
@@ -41,6 +41,7 @@ SOURCE_GROUP("Header Files" FILES ${IDYNTREE_SENSORS_HEADERS})
 set(libraryname idyntree-sensors)
 
 add_library(${libraryname} ${IDYNTREE_SENSORS_SOURCES} ${IDYNTREE_SENSORS_HEADERS} ${IDYNTREE_SENSORS_PRIVATE_INCLUDES})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>"
diff --git a/src/solid-shapes/CMakeLists.txt b/src/solid-shapes/CMakeLists.txt
index 73d80c0b51f..c62371bf560 100644
--- a/src/solid-shapes/CMakeLists.txt
+++ b/src/solid-shapes/CMakeLists.txt
@@ -4,6 +4,7 @@ set(IDYNTREE_SOLID_SHAPES_SOURCES src/InertialParametersSolidShapesHelpers.cpp)
 set(IDYNTREE_SOLID_SHAPES_HEADERS include/iDynTree/InertialParametersSolidShapesHelpers.h)
 
 add_library(${libraryname} ${IDYNTREE_SOLID_SHAPES_HEADERS} ${IDYNTREE_SOLID_SHAPES_SOURCES})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                  "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
diff --git a/src/visualization/CMakeLists.txt b/src/visualization/CMakeLists.txt
index 09aae9b65bc..35bce8b562e 100644
--- a/src/visualization/CMakeLists.txt
+++ b/src/visualization/CMakeLists.txt
@@ -46,6 +46,7 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
 set(libraryname idyntree-visualization)
 add_library(${libraryname} ${iDynTree_visualization_source} ${iDynTree_visualization_header}
                            ${iDynTree_visualization_private_source} ${iDynTree_visualization_private_headers})
+add_library(iDynTree::${libraryname} ALIAS ${libraryname})
 
 set_target_properties(${libraryname} PROPERTIES PUBLIC_HEADER "${iDynTree_visualization_header}")
 
diff --git a/src/yarp/CMakeLists.txt b/src/yarp/CMakeLists.txt
index 1429ae8539e..e0e874d7fe3 100644
--- a/src/yarp/CMakeLists.txt
+++ b/src/yarp/CMakeLists.txt
@@ -30,6 +30,7 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/bin"
 
 
 add_library(idyntree-yarp ${iDynTree_YARP_source} ${iDynTree_YARP_header})
+add_library(iDynTree::idyntree-yarp ALIAS idyntree-yarp)
 
 set_target_properties(idyntree-yarp PROPERTIES PUBLIC_HEADER "${iDynTree_YARP_header}")
 

From 4360142ed4d5dc54ddaec5949043bb31b4efd188 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 10:37:07 +0100
Subject: [PATCH 179/194] Delete .travis.yml

---
 .travis.yml | 98 -----------------------------------------------------
 1 file changed, 98 deletions(-)
 delete mode 100644 .travis.yml

diff --git a/.travis.yml b/.travis.yml
deleted file mode 100644
index 15f930ea9b7..00000000000
--- a/.travis.yml
+++ /dev/null
@@ -1,98 +0,0 @@
-dist: trusty
-language: cpp
-services: docker
-
-os: linux
-
-cache:
-  directories:
-    - $HOME/.ccache
-    - $HOME/Library/Caches/Homebrew
-
-stages:
-  - test # Default stage with job matrix
-  - osx
-
-env:
-  global:
-    - TRAVIS_CMAKE_GENERATOR="Unix Makefiles"
-  matrix:
-    - TRAVIS_BUILD_TYPE="Release" UBUNTU="xenial" VALGRIND_TESTS="ON" COMPILE_BINDINGS="OFF" FULL_DEPS="ON"
-    - TRAVIS_BUILD_TYPE="Debug" UBUNTU="xenial" VALGRIND_TESTS="OFF" COMPILE_BINDINGS="OFF" FULL_DEPS="ON"
-    - TRAVIS_BUILD_TYPE="Release" UBUNTU="bionic" VALGRIND_TESTS="ON" COMPILE_BINDINGS="ON" FULL_DEPS="ON"
-    - TRAVIS_BUILD_TYPE="Debug" UBUNTU="bionic" VALGRIND_TESTS="OFF" COMPILE_BINDINGS="ON" FULL_DEPS="ON"
-
-# ===================
-# STAGE: test (linux)
-# ===================
-
-before_script:
-  - docker pull ubuntu:$UBUNTU
-
-script:
-  - >-
-    docker run -it \
-      -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR \
-      -v $HOME/.ccache:$HOME/.ccache \
-      -w $TRAVIS_BUILD_DIR \
-      --env CC \
-      --env CXX \
-      --env TRAVIS_BUILD_DIR \
-      --env TRAVIS_BUILD_TYPE \
-      --env TRAVIS_CMAKE_GENERATOR \
-      --env VALGRIND_TESTS \
-      --env COMPILE_BINDINGS \
-      --env FULL_DEPS \
-      ubuntu:$UBUNTU \
-      sh .ci/install_debian_and_script.sh
-
-# ==========
-# STAGE: osx
-# ==========
-
-stage_osx:
-  install: &osx_install
-    # Setup ccache
-    - brew update
-    - brew install ccache
-    - export PATH="/usr/local/opt/ccache/libexec:$PATH"
-    # Install dependencies
-    - brew install ace eigen tinyxml swig qt5 gsl pkg-config jpeg tinyxml
-    - export Qt5_DIR=/usr/local/opt/qt5/lib/cmake/Qt5
-  script: &osx_script
-    - cd $TRAVIS_BUILD_DIR/.ci
-    - sh ./script.sh
-
-# ======================
-# BUILD JOBS FROM STAGES
-# ======================
-
-jobs:
-  include:
-    # ---------
-    # STAGE OSX
-    # ---------
-    - &osx_template
-      stage: osx
-      os: osx
-      services:
-      osx_image: xcode9.4
-      before_install: skip
-      install: *osx_install
-      before_script: skip
-      script: *osx_script
-      after_failure: skip
-      after_success: skip
-      after_script: skip
-      compiler: clang
-      env:
-        TRAVIS_CMAKE_GENERATOR="Unix Makefiles"
-        TRAVIS_BUILD_TYPE="Debug"
-        VALGRIND_TESTS="OFF"
-        COMPILE_BINDINGS="OFF"
-        FULL_DEPS="OFF"
-
-
-notifications:
-  email:
-    - pegua1@gmail.com

From 2dc52ea097410b0dfaf7362d77231d565044e67c Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 10:56:18 +0100
Subject: [PATCH 180/194] Cleanup the definition of IDYNTREE_DEPRECATED, and
 disable it for idyntree-core and idyntree-highlevel

---
 src/core/include/iDynTree/Core/Utils.h | 31 ++++++++++----------------
 1 file changed, 12 insertions(+), 19 deletions(-)

diff --git a/src/core/include/iDynTree/Core/Utils.h b/src/core/include/iDynTree/Core/Utils.h
index a5405d0d030..8840562ee11 100644
--- a/src/core/include/iDynTree/Core/Utils.h
+++ b/src/core/include/iDynTree/Core/Utils.h
@@ -20,31 +20,24 @@
  */
 #define IDYNTREE_UNUSED(var) ((void)var)
 
+
+// Note: we set IDYNTREE_DEPRECATED to nothing when compiling idyntree-core and
+// idyntree-high-level because some deprecated functions (mainly related to semantics)
+// need still too be used, as they are going to be removed without any replacement
+// When this functions will be removed as part of iDynTree 2.0, we can remove this special case
+// Furthermore, SWIG has some problems with this attributes, so until we use a recent SWIG version
+// we also disabled them for SWIG
 /**
  * \brief Macro to deprecate functions and methods
  *
  * see https://blog.samat.io/2017/02/27/Deprecating-functions-and-methods-in-Cplusplus/
  */
-// C++14
-#if __cplusplus >= 201402L
-  #if defined(__has_cpp_attribute)
-    #if __has_cpp_attribute(deprecated)
-      #define IDYNTREE_DEPRECATED [[deprecated]]
-      #define IDYNTREE_DEPRECATED_WITH_MSG(msg) [[deprecated(msg)]]
-    #endif
-  #endif
-// Earlier standards
+#if defined(idyntree_core_EXPORTS) || defined(idyntree_high_level_EXPORTS) || defined(SWIG)
+#define IDYNTREE_DEPRECATED
+#define IDYNTREE_DEPRECATED_WITH_MSG(msg)
 #else
-  #if defined(__GNUC__) || defined(__clang__)
-    #define IDYNTREE_DEPRECATED __attribute__((deprecated))
-    #define IDYNTREE_DEPRECATED_WITH_MSG(msg) __attribute__((deprecated(msg)))
-  #elif defined(_MSC_VER)
-    #define IDYNTREE_DEPRECATED __declspec(deprecated)
-    #define IDYNTREE_DEPRECATED_WITH_MSG(msg) __declspec(deprecated(msg))
-  #else
-    #define IDYNTREE_DEPRECATED
-    #define IDYNTREE_DEPRECATED_WITH_MSG(msg)
-  #endif
+#define IDYNTREE_DEPRECATED [[deprecated]]
+#define IDYNTREE_DEPRECATED_WITH_MSG(msg) [[deprecated(msg)]]
 #endif
 
 namespace iDynTree

From 198819cb94f548e64572d85a04160df45d2e443d Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 10:56:51 +0100
Subject: [PATCH 181/194] Deprecate semantics checks

See https://github.com/robotology/idyntree/pull/621
---
 .../iDynTree/Core/AngularForceVector3.h        |  8 +++++++-
 .../iDynTree/Core/AngularMotionVector3.h       |  3 +++
 src/core/include/iDynTree/Core/GeomVector3.h   |  5 +++++
 .../include/iDynTree/Core/LinearForceVector3.h |  2 ++
 .../iDynTree/Core/LinearMotionVector3.h        |  4 ++++
 src/core/include/iDynTree/Core/Position.h      |  2 ++
 src/core/include/iDynTree/Core/Rotation.h      |  2 ++
 .../include/iDynTree/Core/RotationSemantics.h  | 18 +++++++++++++++++-
 src/core/include/iDynTree/Core/SpatialVector.h |  1 +
 src/core/include/iDynTree/Core/Transform.h     |  2 ++
 .../include/iDynTree/Core/TransformSemantics.h |  6 ++++++
 src/core/include/iDynTree/Core/Wrench.h        |  1 -
 12 files changed, 51 insertions(+), 3 deletions(-)

diff --git a/src/core/include/iDynTree/Core/AngularForceVector3.h b/src/core/include/iDynTree/Core/AngularForceVector3.h
index 2f702825015..8ec0f736a72 100644
--- a/src/core/include/iDynTree/Core/AngularForceVector3.h
+++ b/src/core/include/iDynTree/Core/AngularForceVector3.h
@@ -13,6 +13,7 @@
 
 #include <iDynTree/Core/ForceVector3.h>
 #include <iDynTree/Core/PrivateMotionForceVertorAssociations.h>
+#include <iDynTree/Core/Utils.h>
 
 namespace iDynTree
 {
@@ -24,7 +25,8 @@ namespace iDynTree
     /**
      * Class providing the semantics for any angular force vector (torque or angular momentum).
      */
-    class AngularForceVector3Semantics: public ForceVector3Semantics<AngularForceVector3Semantics>
+    class
+    AngularForceVector3Semantics: public ForceVector3Semantics<AngularForceVector3Semantics>
     {
     protected:
         int point;
@@ -34,17 +36,21 @@ namespace iDynTree
          * Constructors:
          */
         inline AngularForceVector3Semantics() {}
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         AngularForceVector3Semantics(int _point, int _body, int _refBody, int _coordinateFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         AngularForceVector3Semantics(const AngularForceVector3Semantics & other);
 
         /**
          * Semantics operations
          * Compute the semantics of the result given the semantics of the operands.
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         bool changePoint(const PositionSemantics & newPoint,
                          const LinearForceVector3Semantics & otherLinear,
                          AngularForceVector3Semantics & resultAngular) const;
 
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         static bool compose(const AngularForceVector3Semantics & op1,
                             const AngularForceVector3Semantics & op2,
                             AngularForceVector3Semantics & result);
diff --git a/src/core/include/iDynTree/Core/AngularMotionVector3.h b/src/core/include/iDynTree/Core/AngularMotionVector3.h
index 3682fc4d2fe..b11c03baa25 100644
--- a/src/core/include/iDynTree/Core/AngularMotionVector3.h
+++ b/src/core/include/iDynTree/Core/AngularMotionVector3.h
@@ -13,6 +13,7 @@
 
 #include <iDynTree/Core/MotionVector3.h>
 #include <iDynTree/Core/PrivateMotionForceVertorAssociations.h>
+#include <iDynTree/Core/Utils.h>
 
 namespace iDynTree
 {
@@ -26,7 +27,9 @@ namespace iDynTree
          * Constructors:
          */
         AngularMotionVector3Semantics();
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         AngularMotionVector3Semantics(int _body, int _refBody, int _coordinateFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         AngularMotionVector3Semantics(const AngularMotionVector3Semantics & other);
     };
 
diff --git a/src/core/include/iDynTree/Core/GeomVector3.h b/src/core/include/iDynTree/Core/GeomVector3.h
index 921dfcea0d3..b6c762f6983 100644
--- a/src/core/include/iDynTree/Core/GeomVector3.h
+++ b/src/core/include/iDynTree/Core/GeomVector3.h
@@ -14,6 +14,7 @@
 #include <iDynTree/Core/VectorFixSize.h>
 #include <iDynTree/Core/Rotation.h>
 #include <iDynTree/Core/PrivateMotionForceVertorAssociations.h>
+#include <iDynTree/Core/Utils.h>
 
 #define GEOMVECTOR3SEMANTICS_TEMPLATE_HDR \
 template <class MotionForceSemanticsT>
@@ -41,7 +42,9 @@ namespace iDynTree
          * Constructors:
          */
         inline GeomVector3Semantics() {}
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         GeomVector3Semantics(int _body, int _refBody, int _coordinateFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         GeomVector3Semantics(const GeomVector3Semantics & other);
 
         void setToUnknown();
@@ -97,7 +100,9 @@ namespace iDynTree
         /**
          * Getters & setters
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         const MotionForceSemanticsT& getSemantics() const;
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setSemantics(MotionForceSemanticsT& _semantics);
 
         /**
diff --git a/src/core/include/iDynTree/Core/LinearForceVector3.h b/src/core/include/iDynTree/Core/LinearForceVector3.h
index 17484df2855..60c32d47471 100644
--- a/src/core/include/iDynTree/Core/LinearForceVector3.h
+++ b/src/core/include/iDynTree/Core/LinearForceVector3.h
@@ -26,7 +26,9 @@ namespace iDynTree
          * Constructors:
          */
         inline LinearForceVector3Semantics() {}
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         LinearForceVector3Semantics(int _body, int _refBody, int _coordinateFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         LinearForceVector3Semantics(const LinearForceVector3Semantics & other);
     };
 
diff --git a/src/core/include/iDynTree/Core/LinearMotionVector3.h b/src/core/include/iDynTree/Core/LinearMotionVector3.h
index 5f200645442..de7de36fa7b 100644
--- a/src/core/include/iDynTree/Core/LinearMotionVector3.h
+++ b/src/core/include/iDynTree/Core/LinearMotionVector3.h
@@ -34,17 +34,21 @@ namespace iDynTree
          * Constructors:
          */
         inline LinearMotionVector3Semantics() {}
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         LinearMotionVector3Semantics(int _point, int _body, int _refBody, int _coordinateFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         LinearMotionVector3Semantics(const LinearMotionVector3Semantics & other);
 
         /**
          * Semantics operations
          * Compute the semantics of the result given the semantics of the operands.
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         bool changePoint(const PositionSemantics & newPoint,
                          const AngularMotionVector3Semantics & otherAngular,
                          LinearMotionVector3Semantics & resultLinear) const;
 
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         static bool compose(const LinearMotionVector3Semantics & op1,
                             const LinearMotionVector3Semantics & op2,
                             LinearMotionVector3Semantics & result);
diff --git a/src/core/include/iDynTree/Core/Position.h b/src/core/include/iDynTree/Core/Position.h
index 2f38de7236e..eb77bf18aef 100644
--- a/src/core/include/iDynTree/Core/Position.h
+++ b/src/core/include/iDynTree/Core/Position.h
@@ -74,11 +74,13 @@ namespace iDynTree
         /**
          * Semantic getter
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         PositionSemantics& getSemantics();
 
         /**
          * Const Semantic getter
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         const PositionSemantics& getSemantics() const;
 
         /**
diff --git a/src/core/include/iDynTree/Core/Rotation.h b/src/core/include/iDynTree/Core/Rotation.h
index d7afe90911c..bb040db97ac 100644
--- a/src/core/include/iDynTree/Core/Rotation.h
+++ b/src/core/include/iDynTree/Core/Rotation.h
@@ -91,11 +91,13 @@ namespace iDynTree
         /**
          * Semantic getter
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         RotationSemantics& getSemantics();
 
         /**
          * Semantic getter
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         const RotationSemantics& getSemantics() const;
 
         /**
diff --git a/src/core/include/iDynTree/Core/RotationSemantics.h b/src/core/include/iDynTree/Core/RotationSemantics.h
index 02ab2932aa7..11a9d1080ad 100644
--- a/src/core/include/iDynTree/Core/RotationSemantics.h
+++ b/src/core/include/iDynTree/Core/RotationSemantics.h
@@ -13,6 +13,8 @@
 
 #include <string>
 
+#include <iDynTree/Core/Utils.h>
+
 namespace iDynTree
 {
     class PositionSemantics;
@@ -22,7 +24,8 @@ namespace iDynTree
      *
      * \ingroup iDynTreeCore
      */
-    class RotationSemantics
+    class
+    RotationSemantics
     {
     protected:
         int orientationFrame;
@@ -54,13 +57,16 @@ namespace iDynTree
          * Constructor: initialize semantics from individual parameters
          *
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         RotationSemantics(int _orientationFrame, int _body, int _refOrientationFrame, int _refBody);
 
         /**
          * Copy constructor: create a RotationSemantics from another RotationSemantics
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         RotationSemantics(const RotationSemantics & other);
 
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setToUnknown();
 
         /**
@@ -68,16 +74,26 @@ namespace iDynTree
          *  Semantics setters and getters.
          */
         ///@{
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         int getOrientationFrame() const;
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         int getBody() const;
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         int getReferenceOrientationFrame() const;
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         int getRefBody() const;
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         int getCoordinateFrame() const;
 
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setOrientationFrame(int _orientationFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setBody(int _body);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setReferenceOrientationFrame(int _refOrientationFrame);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setRefBody(int _refBody);
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         void setCoordinateFrame(int _coordinateFrame);
     ///@}
 
diff --git a/src/core/include/iDynTree/Core/SpatialVector.h b/src/core/include/iDynTree/Core/SpatialVector.h
index 9c19f113f31..f3bba121e3a 100644
--- a/src/core/include/iDynTree/Core/SpatialVector.h
+++ b/src/core/include/iDynTree/Core/SpatialVector.h
@@ -47,6 +47,7 @@ SpatialVectorSemantics<LinearVec3SemanticsT, AngularVec3SemanticsT>
          */
         SpatialVectorSemantics(LinearVec3SemanticsT & linearVec3, AngularVec3SemanticsT & angularVec3);
 
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         bool check_linear2angularConsistency(const LinearVec3SemanticsT & linearVec3, const AngularVec3SemanticsT & angularVec3);
 
         /**
diff --git a/src/core/include/iDynTree/Core/Transform.h b/src/core/include/iDynTree/Core/Transform.h
index d44b889749f..1c7caea3d0e 100644
--- a/src/core/include/iDynTree/Core/Transform.h
+++ b/src/core/include/iDynTree/Core/Transform.h
@@ -134,11 +134,13 @@ namespace iDynTree
         /**
          * Semantic accessor
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         TransformSemantics & getSemantics();
 
         /**
          * Const semantic getter
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         const TransformSemantics & getSemantics() const;
 
         /**
diff --git a/src/core/include/iDynTree/Core/TransformSemantics.h b/src/core/include/iDynTree/Core/TransformSemantics.h
index 2be9adc7327..c4c67ef5ee6 100644
--- a/src/core/include/iDynTree/Core/TransformSemantics.h
+++ b/src/core/include/iDynTree/Core/TransformSemantics.h
@@ -13,6 +13,8 @@
 
 #include <string>
 
+#include <iDynTree/Core/Utils.h>
+
 namespace iDynTree
 {
     class PositionSemantics;
@@ -44,12 +46,14 @@ namespace iDynTree
         /**
          * Constructor: initialize all semantics from a Transform object (create links)
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         TransformSemantics(PositionSemantics & position, RotationSemantics & rotation);
 
 
         /**
          * Get the rotation part of the transform
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         const RotationSemantics & getRotationSemantics() const;
 
         /**
@@ -60,11 +64,13 @@ namespace iDynTree
         /**
          * Set the rotation part of the transform
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         bool setRotationSemantics(const RotationSemantics & rotation);
 
         /**
          * Set the translation part of the transform
          */
+        IDYNTREE_DEPRECATED_WITH_MSG("All iDynTree semantics class and  methods will be removed in iDynTree 2.0")
         bool setPositionSemantics(const PositionSemantics & position);
 
         /**
diff --git a/src/core/include/iDynTree/Core/Wrench.h b/src/core/include/iDynTree/Core/Wrench.h
index 91d1387e633..f0875e2e8e3 100644
--- a/src/core/include/iDynTree/Core/Wrench.h
+++ b/src/core/include/iDynTree/Core/Wrench.h
@@ -20,7 +20,6 @@ namespace iDynTree
      *
      * \ingroup iDynTreeCore
      *
-     * Currently this class does not support semantics.
      */
     class Wrench: public SpatialForceVector
     {

From a1f7ad12fd1da1ddb3e79e673c5e84759ddd8b44 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 11:06:47 +0100
Subject: [PATCH 182/194] Update CHANGELOG with deprecation of semantics class

Furthermore, reiterate that IDYNTREE_USES_KDL is deprecated and is going to be removed.
---
 CHANGELOG.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 70da971d3f1..ab07d6df38b 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -42,6 +42,10 @@ to normalize errors or as initial points of a nonlinear optimization procedure.
 - The changelog has been migrated to the format described in https://keepachangelog.com/en/1.0.0/ .
 - The CMake config files are now installed in ${CMAKE_INSTALL_PREFIX}/lib/cmake/iDynTree also in Windows.
 
+### Deprecated
+- All the classes and methods that end in Semantics are deprecated, and will be removed in iDynTree 2.0, see https://github.com/robotology/idyntree/pull/622 for more info.
+- The CMake option IDYNTREE_USES_KDL and all the classes available when enabling it are deprecated, and will be removed in iDynTree 2.0 .
+
 ### Fixed
 - Fixed missing `DOF_ACCELLERATION` data in dynamic variable cache ordering
 (https://github.com/robotology/idyntree/pull/587)

From 7911c6be167c91695a07026d86c7a139416ab382 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 14:21:52 +0100
Subject: [PATCH 183/194] Update CONTRIBUTING.md with details on GitHub Actions

---
 CONTRIBUTING.md | 13 ++++---------
 1 file changed, 4 insertions(+), 9 deletions(-)

diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
index 46603907274..ef906c4ec13 100644
--- a/CONTRIBUTING.md
+++ b/CONTRIBUTING.md
@@ -24,21 +24,16 @@ In a nutshell, development of new features/bugfixes happens in separated branch.
 ## Continuous Integration 
 We use [https://en.wikipedia.org/wiki/Continuous_integration](Continuous Integration) to make 
 sure that proposed changes in the library do not cause regression. In particular we use 
-[Travis](https://travis-ci.org/robotology/idyntree) for Linux/OS X and [AppVeyor](https://ci.appveyor.com/project/robotology/idyntree) for Windows. 
+[GitHub Actions](https://travis-ci.org/robotology/idyntree) for Continuous Integration on Linux, macOS and Windows. 
+
 In all this build systems the compilation of the tests is enabled by setting to true 
 the `IDYNTREE_COMPILE_TESTS` CMake flag. 
 
-### Travis 
-All the dependencies (also the optional one) are enabled in the Travis build. 
+Almost all the dependencies are enabled in the Linux build. 
 Furthermore on Linux builds the `IDYNTREE_RUN_VALGRIND_TESTS` flag is enabled, to 
 run all the tests also under the [`valgrind`](http://valgrind.org/) tool, to spot 
 any memory-related error, suck of use of initialized memory or memory leak. 
 
-You can find the specific iDynTree configuration used in the AppVeyor build in the [`.travis.yml`](https://github.com/robotology/idyntree/blob/master/.travis.yml) file. 
-
+Some dependencies or options are disabled on Windows and macOS, and you can find the specific iDynTree configuration used in the GitHub Actions build in the [`.github/workflows/ci.yml`](https://github.com/robotology/idyntree/blob/master/.github/workflows/ci.yml) file. 
 
-### AppVeyor 
-For the time being AppVeyor is just building the iDynTree parts that don't depend on 
-[KDL](https://github.com/orocos/orocos_kinematics_dynamics), [YARP](https://github.com/robotology/yarp) or [icub-main](https://github.com/robotology/icub-main).
 
-You can find the specific iDynTree configuration used in the AppVeyor build in the [`appveyor.yml`](https://github.com/robotology/idyntree/blob/master/appveyor.yml) file. 

From 4d454a2b0ec84ccdca2b5192a7ddac085d994dbd Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 14:23:36 +0100
Subject: [PATCH 184/194] Cleanup Travis references in dev faqs

---
 doc/dev/faqs.md | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/doc/dev/faqs.md b/doc/dev/faqs.md
index e85231f5359..5397dedbc69 100644
--- a/doc/dev/faqs.md
+++ b/doc/dev/faqs.md
@@ -25,14 +25,8 @@ test validating the functionality of the MATLAB interface will be automatically
 ## How to run the Valgrind-based tests 
 [Valgrind MemCheck](http://valgrind.org/) is a tool to identify memory related software bugs (use of initialize memory, memory leaks, ...). To automatically run the iDynTree test suite under Valgrind, just enabled the `IDYNTREE_RUN_VALGRIND_TESTS` CMake option (together with the `IDYNTREE_COMPILE_TESTS` option). 
 
-## How to get notified if my code fails to compile or create a failure in some tests 
-iDynTree uses [Travis](https://travis-ci.org/robotology/idyntree) and [AppVeyor](https://ci.appveyor.com/project/robotology/idyntree) Continous Integration (CI) services
-to make sure that all code commited to the repository. To be notified if some of your commits
-failed to pass tests, just sign in [Travis](https://travis-ci.org/) and [AppVeyor](https://ci.appveyor.com/login)
-with your GitHub account, and you will automatically receive notifications if your commits cause a failure. 
-
 ## What are the things that needs to be done before merging a pull request?
-* Check if Travis and AppVeyor compiler and run the test without any failure.  
+* Check if the GitHub Actions jobs compiles and run the test without any failure.  
 
 ## How to add wrap a new class or function with SWIG 
 * Include the new header in [bindings/iDynTree.i](bindings/iDynTree.i) .  For a little more detailed overview, check [doc/generating-idyntree-matlab-bindings.md](../../doc/generating-idyntree-matlab-bindings.md) . 
@@ -41,7 +35,7 @@ with your GitHub account, and you will automatically receive notifications if yo
 * If you add a instantiated template, remember to add it to swig with the `%template` SWIG command. 
 * Regenerate the MATLAB bindings, following the instructions in https://github.com/robotology/idyntree#matlab-bindings-modifications . 
 * (Optional) Add a new matlab test testing the new code in `bindings/matlab/tests`. 
-  * The structure of tests in `bindings/matlab/tests` uses [MOxUnit](https://github.com/MOxUnit/MOxUnit) a unit test framework for Matlab and Octave. Please see the MOxUnit documentation and existing tests for understanding the structure of the tests. As the tests are run by `Travis` also on Octave, please make sure that your tests do not use Matlab-specific features.
+  * The structure of tests in `bindings/matlab/tests` uses [MOxUnit](https://github.com/MOxUnit/MOxUnit) a unit test framework for Matlab and Octave. Please see the MOxUnit documentation and existing tests for understanding the structure of the tests. As the tests are run by `GitHub Actions` also on Octave, please make sure that your tests do not use Matlab-specific features.
 * Run matlab tests with `ctest -VV -R matlab` in the build directory, and check that they are running file. 
 * Commit the updated version of the bindings by adding everything inside the `bindings/matlab` directory, with commit message `[bindings] update matlab bindings`. 
 

From 9453f9624a1ab38c69b9abefb48c16901cae1f53 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 16:07:30 +0100
Subject: [PATCH 185/194] Delete install_debian_and_script.sh

---
 .ci/install_debian_and_script.sh | 8 --------
 1 file changed, 8 deletions(-)
 delete mode 100644 .ci/install_debian_and_script.sh

diff --git a/.ci/install_debian_and_script.sh b/.ci/install_debian_and_script.sh
deleted file mode 100644
index 7a6b1cbbcb4..00000000000
--- a/.ci/install_debian_and_script.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-#!/bin/sh
-set -e
-
-DIR=$(dirname "$(readlink -f "$0")")
-
-sh $DIR/install_debian.sh
-sh $DIR/script.sh
-

From 6235710ca307c9dc17617d8f8a99821e7edf0431 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 16:07:40 +0100
Subject: [PATCH 186/194] Delete install_debian.sh

---
 .ci/install_debian.sh | 27 ---------------------------
 1 file changed, 27 deletions(-)
 delete mode 100644 .ci/install_debian.sh

diff --git a/.ci/install_debian.sh b/.ci/install_debian.sh
deleted file mode 100644
index b503c4a4c2b..00000000000
--- a/.ci/install_debian.sh
+++ /dev/null
@@ -1,27 +0,0 @@
-#!/bin/sh
-set -e
-
-apt-get update
-
-# noninteractive tzdata ( https://stackoverflow.com/questions/44331836/apt-get-install-tzdata-noninteractive )
-export DEBIAN_FRONTEND=noninteractive
-
-# CI specific packages
-apt-get install -y clang valgrind
-
-# Dependencies
-apt-get install -y build-essential libboost-system-dev libboost-filesystem-dev libboost-thread-dev cmake coinor-libipopt-dev liborocos-kdl-dev libeigen3-dev libtinyxml-dev libace-dev libgsl0-dev libopencv-dev libode-dev git swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev libqt5opengl5-dev libqcustomplot-dev libxml2-dev liburdfdom-dev
-
-# Lua
-apt-get install -y liblua5.1-dev lua5.1
-
-# Python
-apt-get install -y python-dev
-
-# Octave
-apt-get install -y liboctave-dev
-
-
-
-
-

From 4d7d598d16c8239531fb2392ffeea791decd10d5 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 16:07:51 +0100
Subject: [PATCH 187/194] Delete script.sh

---
 .ci/script.sh | 40 ----------------------------------------
 1 file changed, 40 deletions(-)
 delete mode 100644 .ci/script.sh

diff --git a/.ci/script.sh b/.ci/script.sh
deleted file mode 100644
index 5ca444b594b..00000000000
--- a/.ci/script.sh
+++ /dev/null
@@ -1,40 +0,0 @@
-#!/bin/sh
-set -e
-
-# Install source deps
-
-# Build  YARP
-git clone https://github.com/robotology/yarp
-cd yarp
-git checkout devel
-mkdir build
-cd build
-cmake -G"${TRAVIS_CMAKE_GENERATOR}" -DCREATE_LIB_MATH:BOOL=ON -DCMAKE_BUILD_TYPE=${TRAVIS_BUILD_TYPE} ..
-cmake --build . --config ${TRAVIS_BUILD_TYPE} --target install
-cd ../..
-
-# Build ICUB
-git clone https://github.com/robotology/icub-main
-cd icub-main
-git checkout devel
-mkdir build
-cd build
-cmake -G"${TRAVIS_CMAKE_GENERATOR}" -DCMAKE_BUILD_TYPE=${TRAVIS_BUILD_TYPE} ..
-cmake --build . --config ${TRAVIS_BUILD_TYPE} --target install
-cd ../..
-
-# Build, test and install iDynTree
-cd $TRAVIS_BUILD_DIR
-mkdir build && cd build
-cmake -G"${TRAVIS_CMAKE_GENERATOR}" -DCMAKE_BUILD_TYPE=${TRAVIS_BUILD_TYPE} -DIDYNTREE_COMPILE_TESTS:BOOL=ON -DIDYNTREE_RUN_VALGRIND_TESTS:BOOL=${VALGRIND_TESTS} -DCODYCO_TRAVIS_CI:BOOL=ON -DIDYNTREE_USES_KDL:BOOL=${FULL_DEPS} -DIDYNTREE_USES_YARP:BOOL=${FULL_DEPS} -DIDYNTREE_USES_ICUB_MAIN:BOOL=${FULL_DEPS}  -DIDYNTREE_USES_PYTHON:BOOL=${COMPILE_BINDINGS} -DIDYNTREE_USES_LUA:BOOL=${COMPILE_BINDINGS} -DIDYNTREE_USES_OCTAVE:BOOL=${COMPILE_BINDINGS} ..
-cmake --build . --config $TRAVIS_BUILD_TYPE
-ctest --output-on-failure --build-config ${TRAVIS_BUILD_TYPE}
-cmake --build . --config ${TRAVIS_BUILD_TYPE} --target install
-
-# Build iDynTree examples
-cd ../examples
-mkdir build
-cd build
-cmake -G"${TRAVIS_CMAKE_GENERATOR}" -DCMAKE_BUILD_TYPE=${TRAVIS_BUILD_TYPE} ..
-cmake --build . --config ${TRAVIS_BUILD_TYPE}
-

From e214aaa6efae5a93df1ef2c7d9bd249ae20a5af4 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 22:32:52 +0100
Subject: [PATCH 188/194] Use std::mutex instead  of deprecated YARP classes

---
 src/tools/idyntree-sole-gui/plugin/ObserverThread.cpp       | 6 +++++-
 src/tools/idyntree-sole-gui/plugin/include/ObserverThread.h | 5 +----
 .../yarprobotstatepublisher/include/robotstatepublisher.h   | 4 +++-
 .../yarprobotstatepublisher/src/robotstatepublisher.cpp     | 4 ++--
 4 files changed, 11 insertions(+), 8 deletions(-)

diff --git a/src/tools/idyntree-sole-gui/plugin/ObserverThread.cpp b/src/tools/idyntree-sole-gui/plugin/ObserverThread.cpp
index 06c0fbaa806..58e4731e7c2 100644
--- a/src/tools/idyntree-sole-gui/plugin/ObserverThread.cpp
+++ b/src/tools/idyntree-sole-gui/plugin/ObserverThread.cpp
@@ -64,7 +64,7 @@ bool getVectorOfStringFromProperty(yarp::os::Property& prop, std::string key, st
     return true;
 }
 
-ObserverThread::ObserverThread(yarp::os::ResourceFinder& config, int period) : RateThread(period), mutex(1)
+ObserverThread::ObserverThread(yarp::os::ResourceFinder& config, int period) : RateThread(period)
 {
     yDebug("ObserverThread running at %g ms.", getRate());
 
@@ -248,6 +248,8 @@ bool ObserverThread::threadInit()
 
 void ObserverThread::run()
 {
+    std::lock_guard<std::mutex> guard(m_mutex);
+
     // Read encoders
     m_iencs->getEncoders(m_measuredEncodersInDeg.data());
 
@@ -432,6 +434,8 @@ void ObserverThread::drawPolygon(QPainter* qpainter, iDynTree::Polygon& poly)
 
 void ObserverThread::draw(QPainter* qpainter, int widthInPixels, int heightInPixels)
 {
+    std::lock_guard<std::mutex> guard(m_mutex);
+
     // The painter draws directly in the primary sole xy plane (the gui need to appropriatly set the world transform)
     double maxXvizInM = 0.03;
     double maxYvizInM = 0.05;
diff --git a/src/tools/idyntree-sole-gui/plugin/include/ObserverThread.h b/src/tools/idyntree-sole-gui/plugin/include/ObserverThread.h
index 8751d407424..cbc34a5fb88 100644
--- a/src/tools/idyntree-sole-gui/plugin/include/ObserverThread.h
+++ b/src/tools/idyntree-sole-gui/plugin/include/ObserverThread.h
@@ -15,7 +15,6 @@
 
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/RateThread.h>
-#include <yarp/os/Semaphore.h>
 #include <yarp/os/Log.h>
 
 #include <yarp/dev/IAnalogSensor.h>
@@ -69,13 +68,11 @@ class ObserverThread : public yarp::os::RateThread
     iDynTree::Direction m_gravityDirection;
     iCub::ctrl::FirstOrderLowPassFilter *filtIMUGravity;    //!< filter the gravity vector received from the IMU
 
-    yarp::os::Semaphore mutex;
-
     int sensorsNum;
     bool mbSimpleDraw;
 
     // Shared data (populated by the run, and used by the draw)
-    yarp::os::Mutex m_mutex;
+    std::mutex m_mutex;
     iDynTree::Position m_comInPrimarySole;
     iDynTree::Position m_desiredComInPrimarySole;
 
diff --git a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
index d6855ef36fc..a979502f984 100644
--- a/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
+++ b/src/tools/yarprobotstatepublisher/include/robotstatepublisher.h
@@ -23,6 +23,8 @@
 #include <iDynTree/KinDynComputations.h>
 
 #include <yarp/rosmsg/sensor_msgs/JointState.h>
+
+#include <mutex>
 #include <memory>
 
 class YARPRobotStatePublisherModule;
@@ -65,7 +67,7 @@ class YARPRobotStatePublisherModule : public yarp::os::RFModule
    yarp::sig::Matrix m_buf4x4;
 
    // Mutex protecting the method across the different threads
-   yarp::os::Mutex m_mutex;
+   std::mutex m_mutex;
 
    // /JointState topic scruscriber
    std::unique_ptr<yarp::os::Node> m_rosNode;
diff --git a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
index 9659d8f8ca0..4b319e860ea 100644
--- a/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
+++ b/src/tools/yarprobotstatepublisher/src/robotstatepublisher.cpp
@@ -172,7 +172,7 @@ bool YARPRobotStatePublisherModule::configure(ResourceFinder &rf)
 /************************************************************/
 bool YARPRobotStatePublisherModule::close()
 {
-    yarp::os::LockGuard guard(m_mutex);
+    std::lock_guard<std::mutex> guard(m_mutex);
 
     // Disconnect the topic subscriber
     if (m_jointStateSubscriber)
@@ -212,7 +212,7 @@ bool YARPRobotStatePublisherModule::updateModule()
 /************************************************************/
 void YARPRobotStatePublisherModule::onRead(yarp::rosmsg::sensor_msgs::JointState &v)
 {
-    yarp::os::LockGuard guard(m_mutex);
+    std::lock_guard<std::mutex> guard(m_mutex);
 
     // If configure was successful, parse the data
     if (m_baseFrameIndex == iDynTree::FRAME_INVALID_INDEX)

From 56cf9014b4fe4c574b3d9385c409c3f79919883b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 22:48:23 +0100
Subject: [PATCH 189/194] Require YARP 3.3 and remove use of deprecated YARP
 functionalities

In particular:
* Remove use of YARP_INCLUDE_DIRS
* Substitute the use of YARP_LIBRARIES with the explicit libraries to link
* Add variable names in yarp_idl_to_dir
---
 CHANGELOG.md                                      |  1 +
 CMakeLists.txt                                    |  7 -------
 cmake/iDynTreeDependencies.cmake                  |  4 ++--
 src/model_io/iKin-kdl/CMakeLists.txt              |  3 ---
 src/model_io/tests/CMakeLists.txt                 |  3 ---
 src/tests/kdl_tests/CMakeLists.txt                |  3 ---
 src/tools/CMakeLists.txt                          |  3 ---
 src/tools/idyntree-sole-gui/plugin/CMakeLists.txt |  4 ----
 src/tools/plotter/CMakeLists.txt                  | 13 ++++++-------
 src/tools/yarprobotstatepublisher/CMakeLists.txt  |  6 +++---
 src/yarp/CMakeLists.txt                           |  7 ++-----
 11 files changed, 14 insertions(+), 40 deletions(-)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index ab07d6df38b..96b9b9ffa2e 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -40,6 +40,7 @@ to normalize errors or as initial points of a nonlinear optimization procedure.
 
 ### Changed
 - The changelog has been migrated to the format described in https://keepachangelog.com/en/1.0.0/ .
+- If the IDYNTREE_USES_YARP option is enabled, the minimum required version of YARP is 3.3 .
 - The CMake config files are now installed in ${CMAKE_INSTALL_PREFIX}/lib/cmake/iDynTree also in Windows.
 
 ### Deprecated
diff --git a/CMakeLists.txt b/CMakeLists.txt
index be73d6bf755..e1fd68aca25 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -42,13 +42,6 @@ if(NOT IDYNTREE_ONLY_DOCS)
     # Find dependecies
     include(iDynTreeDependencies)
 
-    if(IDYNTREE_USES_YARP)
-        find_package(YARP REQUIRED)
-        if (NOT YARP_HAS_MATH_LIB)
-            message(FATAL_ERROR "YARP was found, but no libYARP_math was detected, please recompile yarp")
-        endif()
-    endif()
-
     # Add external libraries that are embedded in iDynTree
     # source tree, if necessary (by default does not adds
     # anything) feel free to check teh logic inside
diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index f8fdf979a9a..36d6af0ad05 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -52,8 +52,8 @@ if (IDYNTREE_USES_ICUB)
   find_package(ICUB REQUIRED)
 endif ()
 
-idyntree_handle_dependency(YARP)
-set(YARP_REQUIRED_VERSION 2.3.62)
+idyntree_handle_dependency(YARP COMPONENTS os dev math rosmsg idl_tools)
+set(YARP_REQUIRED_VERSION 3.3)
 if(IDYNTREE_USES_YARP AND YARP_FOUND)
   if(${YARP_VERSION} VERSION_LESS ${YARP_REQUIRED_VERSION})
     message(FATAL_ERROR "YARP version ${YARP_VERSION} not sufficient, at least version ${YARP_REQUIRED_VERSION} is required.")
diff --git a/src/model_io/iKin-kdl/CMakeLists.txt b/src/model_io/iKin-kdl/CMakeLists.txt
index e4faf51d56a..e29b919ddb9 100644
--- a/src/model_io/iKin-kdl/CMakeLists.txt
+++ b/src/model_io/iKin-kdl/CMakeLists.txt
@@ -8,9 +8,6 @@
 
 project(iDynTree_ModelIO_iKin_KDL CXX)
 
-find_package(YARP REQUIRED)
-find_package(ICUB REQUIRED)
-
 set(IDYNTREE_MODELIO_IKIN_KDL_HEADERS include/iDynTree/ModelIO/iKin_export.hpp)
 
 set(IDYNTREE_MODELIO_IKIN_KDL_SOURCES src/iKin_export.cpp)
diff --git a/src/model_io/tests/CMakeLists.txt b/src/model_io/tests/CMakeLists.txt
index b44767ec133..f32a743fc62 100644
--- a/src/model_io/tests/CMakeLists.txt
+++ b/src/model_io/tests/CMakeLists.txt
@@ -23,9 +23,6 @@ if (IDYNTREE_USES_SYMORO_PAR)
 endif()
 
 if(IDYNTREE_USES_ICUB_MAIN)
-    find_package(YARP REQUIRED)
-    find_package(ICUB REQUIRED)
-    include_directories(SYSTEM ${ICUB_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS})
     add_executable(check_iKin_export_random_chain check_iKin_export_random_chain.cpp)
     target_link_libraries(check_iKin_export_random_chain idyntree-core idyntree-modelio-urdf-kdl idyntree-modelio-ikin-kdl ${YARP_LIBRARIES} iKin)
     add_test(NAME test_check_iKin_export_random_chain COMMAND check_iKin_export_random_chain)
diff --git a/src/tests/kdl_tests/CMakeLists.txt b/src/tests/kdl_tests/CMakeLists.txt
index 1887c223420..f037f645e8b 100644
--- a/src/tests/kdl_tests/CMakeLists.txt
+++ b/src/tests/kdl_tests/CMakeLists.txt
@@ -41,10 +41,7 @@ if(IDYNTREE_USES_ICUB_MAIN)
   # Commented because at the moment it takes a lot of time
   # add_all_components_test(urdf2dh)
   # target_link_libraries(urdf2dh_test iKin idyntree-kdl idyntree-yarp-kdl idyntree-regressors idyntree-modelio-urdf-kdl idyntree-modelio-ikin)
-  find_package(YARP)
-  find_package(ICUB)
   add_executable(urdf2dh_standalone_test urdf2dh_test.cpp)
-  target_include_directories(urdf2dh_standalone_test PUBLIC ${ICUB_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS})
   target_link_libraries(urdf2dh_standalone_test iKin idyntree-kdl idyntree-yarp-kdl idyntree-regressors idyntree-modelio-urdf-kdl idyntree-modelio-ikin-kdl idyntree-testmodels)
 # \todo reenable this test
   #add_test(NAME test_urdf2dh COMMAND urdf2dh_test urdf_icub_test.urdf)
diff --git a/src/tools/CMakeLists.txt b/src/tools/CMakeLists.txt
index 3a574853f9a..f283840d1a3 100644
--- a/src/tools/CMakeLists.txt
+++ b/src/tools/CMakeLists.txt
@@ -41,11 +41,8 @@ if(IDYNTREE_USES_ICUB_MAIN AND IDYNTREE_USES_KDL AND IDYNTREE_USES_SYMORO_PAR)
 endif()
 
 if(IDYNTREE_USES_ICUB_MAIN)
-  find_package(YARP)
-  find_package(ICUB)
   add_definitions(-D_USE_MATH_DEFINES)
   add_executable(urdf2dh urdf2dh)
-  target_include_directories(urdf2dh PUBLIC ${ICUB_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS})
   target_link_libraries(urdf2dh iKin idyntree-core idyntree-regressors  idyntree-model idyntree-icub idyntree-high-level)
   install(TARGETS urdf2dh DESTINATION bin)
 endif()
diff --git a/src/tools/idyntree-sole-gui/plugin/CMakeLists.txt b/src/tools/idyntree-sole-gui/plugin/CMakeLists.txt
index 4094ef75756..9bc0eb2d4bb 100644
--- a/src/tools/idyntree-sole-gui/plugin/CMakeLists.txt
+++ b/src/tools/idyntree-sole-gui/plugin/CMakeLists.txt
@@ -4,10 +4,6 @@
  
 set(CMAKE_INCLUDE_CURRENT_DIR TRUE)
 
-include_directories(${YARP_INCLUDE_DIRS}
-                    ${GSL_INCLUDE_DIRS}
-                    ${ctrlLib_INCLUDE_DIRS})
-
 set(QtiDynTreeSoleGuiPlugin_SRCS
                              qticubskinguiplugin.cpp
                              qticubskinguiplugin_plugin.cpp
diff --git a/src/tools/plotter/CMakeLists.txt b/src/tools/plotter/CMakeLists.txt
index ea09dd57905..2265d784e72 100644
--- a/src/tools/plotter/CMakeLists.txt
+++ b/src/tools/plotter/CMakeLists.txt
@@ -5,15 +5,14 @@ project(iDynTreePlotter)
 find_package(Qt5 COMPONENTS Widgets Charts QUIET)
 if (${Qt5Charts_FOUND})
 
-  find_package(YARP REQUIRED)
-  list(APPEND CMAKE_MODULE_PATH ${YARP_MODULE_PATH})
-  
-  include(YarpIDL)
-  yarp_idl_to_dir("${CMAKE_CURRENT_SOURCE_DIR}/thrifts/chartsService.thrift" 
-                  "${CMAKE_CURRENT_BINARY_DIR}/autogenerated" THRIFT_SOURCES THRIFT_HEADERS THRIFT_INCLUDE_DIRS)
+  yarp_idl_to_dir(INPUT_FILES "${CMAKE_CURRENT_SOURCE_DIR}/thrifts/chartsService.thrift"
+                  OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}/autogenerated"
+                  SOURCES_VAR THRIFT_SOURCES
+                  HEADERS_VAR THRIFT_HEADERS
+                  INCLUDE_DIRS_VAR THRIFT_INCLUDE_DIRS)
 
   add_library(${PROJECT_NAME}-service STATIC ${THRIFT_SOURCES} ${THRIFT_HEADERS})
-  target_include_directories(${PROJECT_NAME}-service SYSTEM PUBLIC ${THRIFT_INCLUDE_DIRS} ${YARP_INCLUDE_DIRS})
+  target_include_directories(${PROJECT_NAME}-service SYSTEM PUBLIC ${THRIFT_INCLUDE_DIRS})
   target_link_libraries(${PROJECT_NAME}-service YARP::YARP_init YARP::YARP_OS YARP::YARP_sig)
   
   
diff --git a/src/tools/yarprobotstatepublisher/CMakeLists.txt b/src/tools/yarprobotstatepublisher/CMakeLists.txt
index 39f0f045bb7..d7e27ccac71 100644
--- a/src/tools/yarprobotstatepublisher/CMakeLists.txt
+++ b/src/tools/yarprobotstatepublisher/CMakeLists.txt
@@ -4,8 +4,6 @@
 
 project(yarprobotstatepublisher)
 
-find_package(YARP COMPONENTS rosmsg REQUIRED)
-
 add_executable(${PROJECT_NAME}
     include/robotstatepublisher.h
     src/main.cpp
@@ -14,7 +12,9 @@ add_executable(${PROJECT_NAME}
 target_include_directories(${PROJECT_NAME} PRIVATE ${PROJECT_SOURCE_DIR}/include)
 
 target_link_libraries(${PROJECT_NAME}
-    ${YARP_LIBRARIES}
+    YARP::YARP_init
+    YARP::YARP_os
+    YARP::YARP_dev
     YARP::YARP_rosmsg
     idyntree-high-level
     idyntree-yarp)
diff --git a/src/yarp/CMakeLists.txt b/src/yarp/CMakeLists.txt
index f496179616b..a0ce0942b71 100644
--- a/src/yarp/CMakeLists.txt
+++ b/src/yarp/CMakeLists.txt
@@ -33,13 +33,10 @@ set_target_properties(idyntree-yarp PROPERTIES PUBLIC_HEADER "${iDynTree_YARP_he
 target_include_directories(idyntree-yarp PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                 "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
 
-target_include_directories(idyntree-yarp INTERFACE ${YARP_INCLUDE_DIRS})
-
-target_include_directories(idyntree-yarp PRIVATE SYSTEM ${YARP_INCLUDE_DIRS})
-
 target_link_libraries(idyntree-yarp idyntree-core
                                     idyntree-model
-                                    ${YARP_LIBRARIES})
+                                    YARP::YARP_os
+                                    YARP::YARP_math)
                                     
 target_compile_options(idyntree-yarp PRIVATE ${IDYNTREE_WARNING_FLAGS})
 

From a6f1f8e1fe30dcee09c6e9e50ba0747aacc621c3 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 8 Jan 2020 23:51:40 +0100
Subject: [PATCH 190/194] Remove use of YARP_LIBRARIES

---
 examples/KinDynComputationsWithEigen/CMakeLists.txt           | 1 +
 examples/fixedBaseGravityCompensation/CMakeLists.txt          | 2 +-
 examples/icubArmRegressor/CMakeLists.txt                      | 2 +-
 src/legacy/icub-kdl/CMakeLists.txt                            | 3 ++-
 src/legacy/icub-kdl/tests/CMakeLists.txt                      | 2 +-
 src/legacy/kdl/examples/subtree_base_regressor/CMakeLists.txt | 2 +-
 src/legacy/yarp-kdl/CMakeLists.txt                            | 2 +-
 src/model_io/iKin-kdl/CMakeLists.txt                          | 2 +-
 src/model_io/tests/CMakeLists.txt                             | 4 ++--
 src/tests/kdl_tests/CMakeLists.txt                            | 2 +-
 src/tests/yarp_kdl_consistency/CMakeLists.txt                 | 2 +-
 11 files changed, 13 insertions(+), 11 deletions(-)

diff --git a/examples/KinDynComputationsWithEigen/CMakeLists.txt b/examples/KinDynComputationsWithEigen/CMakeLists.txt
index ad040a59c86..250bc5a47b4 100644
--- a/examples/KinDynComputationsWithEigen/CMakeLists.txt
+++ b/examples/KinDynComputationsWithEigen/CMakeLists.txt
@@ -20,6 +20,7 @@ source_group("Source Files" FILES ${folder_source})
 add_executable(${PROJECT_NAME} ${folder_source})
 
 target_include_directories(${PROJECT_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR})
+message(STATUS "iDynTree_LIBRARIES : ${iDynTree_LIBRARIES}")
 target_link_libraries(${PROJECT_NAME} ${iDynTree_LIBRARIES})
 
 
diff --git a/examples/fixedBaseGravityCompensation/CMakeLists.txt b/examples/fixedBaseGravityCompensation/CMakeLists.txt
index 7f39afeee1f..be6f6518fb0 100644
--- a/examples/fixedBaseGravityCompensation/CMakeLists.txt
+++ b/examples/fixedBaseGravityCompensation/CMakeLists.txt
@@ -17,7 +17,7 @@ source_group("Source Files" FILES ${folder_source})
 add_executable(${PROJECT_NAME} ${folder_source})
 
 target_include_directories(${PROJECT_NAME} PUBLIC ${YARP_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}
+target_link_libraries(${PROJECT_NAME} YARP::YARP_os YARP::YARP_math
                                      ${iDynTree_LIBRARIES})
 
 
diff --git a/examples/icubArmRegressor/CMakeLists.txt b/examples/icubArmRegressor/CMakeLists.txt
index e56b2f6d458..011d67ff3bb 100644
--- a/examples/icubArmRegressor/CMakeLists.txt
+++ b/examples/icubArmRegressor/CMakeLists.txt
@@ -17,7 +17,7 @@ source_group("Source Files" FILES ${folder_source})
 add_executable(${PROJECT_NAME} ${folder_source})
 
 target_include_directories(${PROJECT_NAME} PUBLIC ${YARP_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}
+target_link_libraries(${PROJECT_NAME} YARP::YARP_os YARP::YARP_math
                                       ${iDynTree_LIBRARIES})
 
 
diff --git a/src/legacy/icub-kdl/CMakeLists.txt b/src/legacy/icub-kdl/CMakeLists.txt
index 68f04b3f2ab..d4a1e1e0692 100644
--- a/src/legacy/icub-kdl/CMakeLists.txt
+++ b/src/legacy/icub-kdl/CMakeLists.txt
@@ -43,7 +43,8 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
 
 target_link_libraries(${libraryname} LINK_PUBLIC  skinDynLib iKin iDyn
                                             ${GSL_LIBRARIES}
-                                            ${YARP_LIBRARIES}
+                                            YARP::YARP_os
+                                            YARP::YARP_math
                                             idyntree-core
                                             idyntree-sensors
                                             idyntree-modelio-urdf-kdl
diff --git a/src/legacy/icub-kdl/tests/CMakeLists.txt b/src/legacy/icub-kdl/tests/CMakeLists.txt
index 783abfe3e07..1b1698486d9 100644
--- a/src/legacy/icub-kdl/tests/CMakeLists.txt
+++ b/src/legacy/icub-kdl/tests/CMakeLists.txt
@@ -11,7 +11,7 @@ macro(add_icub_test testName)
     set(testname   test_${testName})
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-core idyntree-high-level idyntree-yarp-kdl idyntree-icub-kdl ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES} idyntree-testmodels)
+    target_link_libraries(${testbinary} idyntree-core idyntree-high-level idyntree-yarp-kdl idyntree-icub-kdl  YARP::YARP_os YARP::YARP_math ${orocos_kdl_LIBRARIES} idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/legacy/kdl/examples/subtree_base_regressor/CMakeLists.txt b/src/legacy/kdl/examples/subtree_base_regressor/CMakeLists.txt
index 8d5541f39cf..f74dba2144a 100644
--- a/src/legacy/kdl/examples/subtree_base_regressor/CMakeLists.txt
+++ b/src/legacy/kdl/examples/subtree_base_regressor/CMakeLists.txt
@@ -38,7 +38,7 @@ endforeach(header_file ${folder_header})
 if (folder_source)
   add_executable(subtree_base_regressor ${folder_source} ${folder_header})
   target_link_libraries(subtree_base_regressor ${orocos_kdl_LIBRARIES}
-                                                 ${YARP_LIBRARIES} ${ICUB_LIBRARIES}
+                                                 YARP::YARP_os YARP::YARP_math${ICUB_LIBRARIES}
                                                 ${iDynTree_LIBRARIES})
 else (folder_source)
   message(FATAL_ERROR "No source code files found. Please add something")
diff --git a/src/legacy/yarp-kdl/CMakeLists.txt b/src/legacy/yarp-kdl/CMakeLists.txt
index 6bd038a8967..68545543507 100644
--- a/src/legacy/yarp-kdl/CMakeLists.txt
+++ b/src/legacy/yarp-kdl/CMakeLists.txt
@@ -49,7 +49,7 @@ endif()
 
 target_link_libraries(idyntree-yarp-kdl LINK_PUBLIC
                                             ${GSL_LIBRARIES}
-                                            ${YARP_LIBRARIES}
+                                            YARP::YARP_os YARP::YARP_math
                                             idyntree-core
                                             idyntree-sensors
                                             idyntree-modelio-urdf-kdl
diff --git a/src/model_io/iKin-kdl/CMakeLists.txt b/src/model_io/iKin-kdl/CMakeLists.txt
index e29b919ddb9..a943385c60f 100644
--- a/src/model_io/iKin-kdl/CMakeLists.txt
+++ b/src/model_io/iKin-kdl/CMakeLists.txt
@@ -26,7 +26,7 @@ target_include_directories(${libraryname} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURR
                                                  ${YARP_INCLUDE_DIRS}
                                           PRIVATE ${EIGEN3_INCLUDE_DIR})
 
-target_link_libraries(${libraryname} idyntree-core iKin ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES})
+target_link_libraries(${libraryname} idyntree-core iKin YARP::YARP_os YARP::YARP_math ${orocos_kdl_LIBRARIES})
 
 set_property(TARGET ${libraryname} PROPERTY PUBLIC_HEADER ${IDYNTREE_MODELIO_IKIN_KDL_HEADERS})
 
diff --git a/src/model_io/tests/CMakeLists.txt b/src/model_io/tests/CMakeLists.txt
index f32a743fc62..87da9a0c69a 100644
--- a/src/model_io/tests/CMakeLists.txt
+++ b/src/model_io/tests/CMakeLists.txt
@@ -24,14 +24,14 @@ endif()
 
 if(IDYNTREE_USES_ICUB_MAIN)
     add_executable(check_iKin_export_random_chain check_iKin_export_random_chain.cpp)
-    target_link_libraries(check_iKin_export_random_chain idyntree-core idyntree-modelio-urdf-kdl idyntree-modelio-ikin-kdl ${YARP_LIBRARIES} iKin)
+    target_link_libraries(check_iKin_export_random_chain idyntree-core idyntree-modelio-urdf-kdl idyntree-modelio-ikin-kdl YARP::YARP_os YARP::YARP_math iKin)
     add_test(NAME test_check_iKin_export_random_chain COMMAND check_iKin_export_random_chain)
 
 endif()
 
 #check iKin Denavit Hartenberg parameters export
 #add_executable(check_iKin_export_random_chain check_iKin_export_random_chain.cpp)
-#target_link_libraries(check_iKin_export_random_chain ${kdl_codyco_LIBRARIES} ${YARP_LIBRARIES} ${ICUB_LIBRARIES} kdl-format-io)
+#target_link_libraries(check_iKin_export_random_chain ${kdl_codyco_LIBRARIES} YARP::YARP_os YARP::YARP_math ${ICUB_LIBRARIES} kdl-format-io)
 
 
 #In Release mode, don't compile the HRP2 Regressor model because it takes a lot of time
diff --git a/src/tests/kdl_tests/CMakeLists.txt b/src/tests/kdl_tests/CMakeLists.txt
index f037f645e8b..28d2654562a 100644
--- a/src/tests/kdl_tests/CMakeLists.txt
+++ b/src/tests/kdl_tests/CMakeLists.txt
@@ -17,7 +17,7 @@ macro(add_all_components_test testName)
     set(testname   test_${testName})
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary} idyntree-core idyntree-high-level-kdl idyntree-yarp-kdl ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES} idyntree-testmodels)
+    target_link_libraries(${testbinary} idyntree-core idyntree-high-level-kdl idyntree-yarp-kdl YARP::YARP_os YARP::YARP_math ${orocos_kdl_LIBRARIES} idyntree-testmodels)
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)
diff --git a/src/tests/yarp_kdl_consistency/CMakeLists.txt b/src/tests/yarp_kdl_consistency/CMakeLists.txt
index a116e0dc468..bb9d8a67ccb 100644
--- a/src/tests/yarp_kdl_consistency/CMakeLists.txt
+++ b/src/tests/yarp_kdl_consistency/CMakeLists.txt
@@ -12,7 +12,7 @@ macro(add_yarp_kdl_consistency_test testName)
     set(testname   ${testName}ConsistencyTest)
     add_executable(${testbinary} ${testsrc})
     target_include_directories(${testbinary} PRIVATE ${EIGEN3_INCLUDE_DIR})
-    target_link_libraries(${testbinary}  idyntree-modelio-urdf-kdl idyntree-yarp-kdl idyntree-yarp idyntree-core idyntree-kdl idyntree-model ${YARP_LIBRARIES} ${orocos_kdl_LIBRARIES})
+    target_link_libraries(${testbinary}  idyntree-modelio-urdf-kdl idyntree-yarp-kdl idyntree-yarp idyntree-core idyntree-kdl idyntree-model YARP::YARP_os YARP::YARP_math ${orocos_kdl_LIBRARIES})
     add_test(NAME ${testname} COMMAND ${testbinary})
 
     if(IDYNTREE_RUN_VALGRIND_TESTS)

From 96f2a811d75c3a8e3b344d0059ddc1bcf3367f37 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 9 Jan 2020 09:41:35 +0100
Subject: [PATCH 191/194] Support the use of dependencies made available via
 add_subdirectory

If iDynTree is included as part of a bigger project via add_subdirectory,
it is possible that some of its dependencies are included via add_subdirectory as well.
For this reason, this PR adds the logic to avoid searching via find_package for
a CMake package, if the relevant target are already defined.
---
 cmake/iDynTreeDependencies.cmake | 61 ++++++++++++++++++++++----------
 1 file changed, 43 insertions(+), 18 deletions(-)

diff --git a/cmake/iDynTreeDependencies.cmake b/cmake/iDynTreeDependencies.cmake
index 36d6af0ad05..23b12991a82 100644
--- a/cmake/iDynTreeDependencies.cmake
+++ b/cmake/iDynTreeDependencies.cmake
@@ -4,16 +4,31 @@
 
 #########################################################################
 # Enable/disable dependencies
-# DO_NOT_SILENTLY_SEARCH: Do not search for the package to set the default
-#                         value of IDYNTREE_USES_<dep> option, but just set
-#                         it to OFF 
+# Positional argument: CMake package name
+# Options:
+# MINIMUM_VERSION <version> : Minimum version required for the package
+# COMPONENTS <comp1> ...    : Components that needs to be find for the dependency
+# DO_NOT_SILENTLY_SEARCH    : Do not search for the package to set the default
+#                             value of IDYNTREE_USES_<dep> option, but just set
+#                             it to OFF
+# MAIN_TARGET               : If the specified target exists, then the package is
+#                             is not searched via find_package because it means that
+#                             iDynTree has been added to a bigger project via add_subdirectory,
+#                             and the required dependency has already been defined via add_subdirectory
 macro(idyntree_handle_dependency package)
   set(options DO_NOT_SILENTLY_SEARCH)
-  set(singleValueArgs MINIMUM_VERSION)
+  set(singleValueArgs MINIMUM_VERSION MAIN_TARGET)
   set(multiValueArgs COMPONENTS)
   cmake_parse_arguments(IHD "${options}" "${singleValueArgs}" "${multiValueArgs}" ${ARGN})
   string(TOUPPER ${package} PKG)
-  if (NOT IHD_DO_NOT_SILENTLY_SEARCH)
+  # Handle MAIN_TARGET option
+  set(IHD_DEP_TARGET_IS_ALREADY_DEFINED FALSE)
+  if(IHD_MAIN_TARGET)
+    if(TARGET ${IHD_MAIN_TARGET})
+      set(IHD_DEP_TARGET_IS_ALREADY_DEFINED TRUE)
+    endif()
+  endif()
+  if (NOT IHD_DO_NOT_SILENTLY_SEARCH AND NOT IHD_DEP_TARGET_IS_ALREADY_DEFINED)
     if (IHD_COMPONENTS)
       find_package(${package} ${IHD_MINIMUM_VERSION} QUIET COMPONENTS ${IHD_COMPONENTS})
     else ()
@@ -21,10 +36,15 @@ macro(idyntree_handle_dependency package)
     endif ()
     set(IDYNTREE_USES_${PKG}_DEFAULT ${${package}_FOUND})
   else ()
-    set(IDYNTREE_USES_${PKG}_DEFAULT FALSE)
+    # If the target is defined, the dependency is enabled unless IDYNTREE_USES_${PKG} is explicitly set to FALSE
+    if (IHD_DEP_TARGET_IS_ALREADY_DEFINED)
+      set(IDYNTREE_USES_${PKG}_DEFAULT TRUE)
+    else ()
+      set(IDYNTREE_USES_${PKG}_DEFAULT FALSE)
+    endif()
   endif ()
   option(IDYNTREE_USES_${PKG} "Build the part of iDynTree that depends on package ${package}" ${IDYNTREE_USES_${PKG}_DEFAULT})
-  if (IDYNTREE_USES_${PKG})
+  if (IDYNTREE_USES_${PKG} AND NOT IHD_DEP_TARGET_IS_ALREADY_DEFINED)
     if (IHD_COMPONENTS)
       find_package(${package} ${IHD_MINIMUM_VERSION} COMPONENTS ${IHD_COMPONENTS} REQUIRED)
     else ()
@@ -34,8 +54,13 @@ macro(idyntree_handle_dependency package)
 endmacro ()
 
 # Eigen is compulsory (minimum version 3.2.92)
-find_package(Eigen3 3.2.92 REQUIRED)
-find_package(LibXml2 REQUIRED)
+if(NOT TARGET Eigen::Eigen3)
+  find_package(Eigen3 3.2.92 REQUIRED)
+endif()
+
+if(NOT TARGET LibXml2::LibXml2)
+  find_package(LibXml2 REQUIRED)
+endif()
 
 # For orocos_kdl we have custom logic, because we want to set it to FALSE by default
 option(IDYNTREE_USES_KDL "Build the part of iDynTree that depends on package orocos_kdl" FALSE)
@@ -46,13 +71,7 @@ if (IDYNTREE_USES_KDL)
   find_package(orocos_kdl REQUIRED)
 endif ()
 
-find_package(ICUB QUIET)
-option(IDYNTREE_USES_ICUB_MAIN "Build the part of iDynTree that depends on package ICUB" ${ICUB_FOUND})
-if (IDYNTREE_USES_ICUB)
-  find_package(ICUB REQUIRED)
-endif ()
-
-idyntree_handle_dependency(YARP COMPONENTS os dev math rosmsg idl_tools)
+idyntree_handle_dependency(YARP COMPONENTS os dev math rosmsg idl_tools MAIN_TARGET YARP::YARP_os)
 set(YARP_REQUIRED_VERSION 3.3)
 if(IDYNTREE_USES_YARP AND YARP_FOUND)
   if(${YARP_VERSION} VERSION_LESS ${YARP_REQUIRED_VERSION})
@@ -60,10 +79,16 @@ if(IDYNTREE_USES_YARP AND YARP_FOUND)
   endif()
 endif()
 
+find_package(ICUB QUIET)
+option(IDYNTREE_USES_ICUB_MAIN "Build the part of iDynTree that depends on package ICUB" ${ICUB_FOUND})
+if (IDYNTREE_USES_ICUB)
+  find_package(ICUB REQUIRED)
+endif ()
+
 idyntree_handle_dependency(IPOPT)
 idyntree_handle_dependency(Irrlicht)
 idyntree_handle_dependency(Qt5 COMPONENTS Qml Quick Widgets)
-idyntree_handle_dependency(OsqpEigen)
+idyntree_handle_dependency(OsqpEigen MAIN_TARGET OsqpEigen::OsqpEigen)
 idyntree_handle_dependency(ALGLIB DO_NOT_SILENTLY_SEARCH)
 set(ALGLIB_REQUIRED_VERSION 3.14.0)
 if(IDYNTREE_USES_ALGLIB AND ALGLIB_FOUND)
@@ -73,4 +98,4 @@ if(IDYNTREE_USES_ALGLIB AND ALGLIB_FOUND)
 endif()
 idyntree_handle_dependency(WORHP)
 # Workaround for https://github.com/robotology/idyntree/issues/599
-idyntree_handle_dependency(ASSIMP DO_NOT_SILENTLY_SEARCH)
+idyntree_handle_dependency(ASSIMP DO_NOT_SILENTLY_SEARCH MAIN_TARGET assimp::assimp)

From b0c4423096b2ede7b65f5ce1d9ebb05b4b578d9f Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Sun, 12 Jan 2020 16:55:32 +0100
Subject: [PATCH 192/194] Add libqt5charts5-dev package in GitHub Action

To compile the idyntree-plotter utility
---
 .github/workflows/ci.yml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index a98aab1c807..9501e0806b8 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -46,7 +46,7 @@ jobs:
       run: |
         sudo apt-get update
         sudo apt-get install git build-essential cmake libace-dev coinor-libipopt-dev  libboost-system-dev libboost-filesystem-dev \
-                             libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev \
+                             libboost-thread-dev liborocos-kdl-dev libeigen3-dev swig qtbase5-dev qtdeclarative5-dev qtmultimedia5-dev libqt5charts5-dev \
                              libxml2-dev liburdfdom-dev libtinyxml-dev liburdfdom-dev liboctave-dev python-dev valgrind libassimp-dev
         
     - name: Source-based Dependencies [Windows] 

From 5801e384e401da54a5414817307c774ef2bf993a Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 13 Jan 2020 10:07:32 +0100
Subject: [PATCH 193/194] Remove thrift from the include path of
 thrift-generated files

It is not clear why this is necessary, see:
* https://github.com/robotology/yarp/issues/1092
* https://github.com/robotology/yarp/issues/2142

However, the new style (without the prepended "thrift") seems to be working fine on both YARP 3.3 and master,
so given that iDynTree 1.0 now requires YARP 3.3 as the minimum version, I think it is ok to just fix the issue by
removing the "thrift" directory.
---
 src/tools/plotter/include/ChartsManager.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/tools/plotter/include/ChartsManager.h b/src/tools/plotter/include/ChartsManager.h
index dc6d97e686b..3a49040d396 100644
--- a/src/tools/plotter/include/ChartsManager.h
+++ b/src/tools/plotter/include/ChartsManager.h
@@ -2,8 +2,8 @@
 #define IDYNTREE_PLOTTER_CHARTSMANAGER_H
 
 
-#include "thrifts/ChartsService.h"
-#include "thrifts/RealTimeStreamData.h"
+#include <ChartsService.h>
+#include <RealTimeStreamData.h>
 #include <yarp/os/Port.h>
 #include <yarp/os/BufferedPort.h>
 #include <yarp/sig/Vector.h>

From 0c8ee1e3367fe0d136e089888870ed2bdaa35da6 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 14 Jan 2020 12:41:58 +0100
Subject: [PATCH 194/194] Bump version to 1.0.0

---
 CMakeLists.txt | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e1fd68aca25..c333f82e958 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -23,9 +23,9 @@ endif()
 
 set(VARS_PREFIX "iDynTree")
 
-set(${VARS_PREFIX}_MAJOR_VERSION 0)
-set(${VARS_PREFIX}_MINOR_VERSION 11)
-set(${VARS_PREFIX}_PATCH_VERSION 105)
+set(${VARS_PREFIX}_MAJOR_VERSION 1)
+set(${VARS_PREFIX}_MINOR_VERSION 0)
+set(${VARS_PREFIX}_PATCH_VERSION 0)
 set(${VARS_PREFIX}_VERSION ${${VARS_PREFIX}_MAJOR_VERSION}.${${VARS_PREFIX}_MINOR_VERSION}.${${VARS_PREFIX}_PATCH_VERSION})
 
 # Pick up our CMake scripts - they are all in the cmake subdirectory.